ROS_x_STREAM some final debug
Fix destructor
Debug changed, planning is okay now... really basic though. Most problems are with motion primitives, interpolation paths, ... parameters!