MoveIt plan fails after attaching an object, but not from Rviz or after restarting requesting node

edit

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 ...