diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 4ccf04fd82..a0d9c6d8f8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1657,8 +1657,6 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache const EigenSTL::vector_Isometry3d& poses = attached_body->getGlobalCollisionBodyTransforms(); const std::string& name = attached_body->getName(); - robot_state_->clearAttachedBody(name); - if (world_->hasObject(name)) ROS_WARN_NAMED(LOGNAME, "The collision world already has an object with the same name as the body about to be detached. " @@ -1670,6 +1668,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache ROS_DEBUG_NAMED(LOGNAME, "Detached object '%s' from link '%s' and added it back in the collision world", name.c_str(), object.link_name.c_str()); } + + robot_state_->clearAttachedBody(name); } if (!attached_bodies.empty() || object.object.id.empty()) return true;