MoveIt! planning not continue after successful planning?
I was following this movegroup interface tutorial ( http://docs.ros.org/hydro/api/pr2_mov... ) on my own 7DOF robot. It has successfully found a solution but the program is not continuing to the next line of code. (See code below, so it is not printing out "Visualizing ..." in terminal)
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan); // stuck here
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"SUCCESSFUL":"FAILED");
sleep(5.0);
The last displayed lines are:
[ INFO] [1459836629.415696549]: Solution found in 0.395740 seconds
[ INFO] [1459835061.553672913]: SimpleSetup: Path simplification took 0.029544 seconds and changed from 3 to 2 states
Anyone has any idea why this is happening? (I can see in RViz that the planner definitely found the path, but it won't let me continue so I can publish it to "/move_group/display_planned_path"). Either something is blocking the process or is waiting for some messages.
I found a possible cause: It is stuck at waitForResult() in the plan() function. The problem is similar to https://groups.google.com/forum/#!top... but I'm running group.plan(my_plan) inside main function.
Anyone has any clue how to get out of this waitForResult() function. (it is waiting for cur_simple_state_ == SimpleGoalState::DONE according to the source code)