* Refactored API. Removed init/fini functions for types that do not necessarily need allocation on the heap. * Added unit tests for implementation.
* Fix buggy if-conditions in transition functions. * Bugfix: incease number of states by one * Cleanup CMakeLists.txt and package.xml * Move goal state machine implementation details from header to C file