MoveIt plan fails after attaching an object, but not from Rviz or after restarting requesting node
I am implementing a tool change procedure by attaching a collision object to the robot's gripper. However, after attaching the object, the planner fails to sample any valid states for the goal tree. More strangely, if I leave the scene untouched, restart the node without re-attaching the object, and execute the same planning requests, they succeed. Launching the planning requests from Rviz works, and goToNamedPose (see code below) also works, strangely.
I figure that means that the move group itself is intact, but something pollutes my code that causes the planning to fail. What is it? What could I do to debug further? Setting the rqt_logger_level to debug did not turn up any useful messages from the move group.
I have tried to condense the code to the pertinent parts. I can send a dockerized sandbox to anyone who is interested in checking this out, it just contains other messy code that I wouldn't want to upload needlessly.
Any pointers or ideas are appreciated.
main()
{
// ROS_INFO("Testing the tool mounting.");
o2as_skill_server.equipTool("b_bot", "tool_m5");
o2as_skill_server.goToNamedPose("home_b", "b_bot");
geometry_msgs::PoseStamped target_ps = makePoseStamped();
target_ps.header.frame_id = "tray_1";
target_ps.pose.position.z = .02;
// We ignore the orientation of the (they always point down) and assign an appropriate orientation
// TODO: How to find a good pose for the robot automatically? Launch a planning request to MoveIt?
target_ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -(110.0/180.0 *M_PI)); // Z-axis pointing up.
std::string link_name = "b_bot_tool_m5_tip_link";
std::string robot_name = "b_bot";
moveToCartPosePTP(target_ps, robot_name, true, link_name);
o2as_skill_server.unequipTool("b_bot");
ROS_INFO("Done.");
}
bool equipTool(std::string robot_name, std::string tool_id) {return equipUnequipTool(robot_name, tool_id, "equip");}
bool unequipTool(std::string robot_name) {return equipUnequipTool(robot_name, held_tool_, "unequip");}
bool equipUnequipTool(std::string robot_name, std::string tool_id, std::string equip_or_unequip)
{
bool equip = (equip_or_unequip == "equip");
bool unequip = (equip_or_unequip == "unequip");
// Plan & execute motion to in front of tool holder
geometry_msgs::PoseStamped ps_approach, ps_tool_holder;
ps_approach.header.frame_id = tool_id + "_link";
ps_approach.pose.position.y = .1;
ps_approach.pose.position.z = .017;
ps_approach.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -M_PI / 2);
ROS_INFO("Moving to tool approach pose PTP.");
moveToCartPosePTP(ps_approach, robot_name);
if (equip) {
openGripper(robot_name);
ROS_INFO("Spawning tool. Waiting for a while to be safe.");
spawnTool(tool_id);
}
// Plan & execute linear motion to the tool change position
ps_tool_holder = ps_approach;
ps_tool_holder.pose.position.y = -0.03;
ROS_INFO("Moving to pose in tool holder LIN.");
moveToCartPoseLIN(ps_tool_holder, robot_name);
// Close gripper, attach the tool object to the gripper in the Planning Scene.
if (equip)
{
closeGripper(robot_name);
attachTool(tool_id, robot_name);
}
else if (unequip)
{
openGripper(robot_name);
detachTool(tool_id, robot_name);
}
// Plan & execute linear motion away from the tool change position
ROS_INFO("Moving back to tool approach pose LIN.");
moveToCartPoseLIN(ps_approach, robot_name);
if (unequip) despawnTool(tool_id);
return true;
}
bool moveToCartPosePTP(geometry_msgs::PoseStamped pose, std::string robot_name, bool wait, std::string end_effector_link)
{
moveit::planning_interface::MoveGroupInterface::Plan myplan;
moveit::planning_interface::MoveItErrorCode
success_plan = moveit_msgs::MoveItErrorCodes::FAILURE,
motion_done = moveit_msgs::MoveItErrorCodes::FAILURE;
moveit::planning_interface::MoveGroupInterface* group_pointer;
group_pointer = robotNameToMoveGroup(robot_name);
group_pointer->setStartStateToCurrentState();
group_pointer->setEndEffectorLink(end_effector_link);
group_pointer->setPoseTarget(pose);
ROS_DEBUG_STREAM("Planning motion for robot " << robot_name << " and EE link " << end_effector_link ...