Moveit gets stuck after planning
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