Moveit gets stuck after planning

edit

Hi everyone,

I currently want to send a joint-space goal to my 6DOF arm. move_point is a pointer to my move_group. The command comes from a GUI I wrote and the callback looks like this:

void goal_joint_callback(mybot::joint_command msg){
     ROS_INFO("debug1");
     std::vector<double> joint_group_positions(6);
     joint_group_positions[0] = msg.joint1;  // radians
     joint_group_positions[1] = msg.joint2;
     joint_group_positions[2] = msg.joint3; 
     joint_group_positions[3] = msg.joint4; 
     joint_group_positions[4] = msg.joint5;
     joint_group_positions[5] = msg.joint6;
     ROS_INFO("debug2");
     move_point->setJointValueTarget(joint_group_positions);
     ROS_INFO("debug3");
     bool success = move_point->plan(*current_plan);
     ROS_INFO("debug4");
     ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
 }

The goal gets visualized quite nice in rviz and until then everything looks ok.

In my output terminal I can see "debug 1,2,3" but not "debug 4": I am also unable to send additional goals after the first one. I cant really tell why, because the same lines of code (bool success = move_point->plan(*current_plan);) do work at another place with the cartesian path.

Thank you in advance,

Felix