Ports moveit1 #3689 (backport #3357) (#3364)

(cherry picked from commit e2b24f5)

Co-authored-by: Mark Johnson <104826595+rr-mark@users.noreply.github.com>

moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp

@@ -203,15 +203,6 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(
 {
   RCLCPP_INFO(LOGGER, "Planning request received for MoveGroupSequenceAction action.");
 
-  // lock the scene so that it does not modify the world representation while
-  // diff() is called
-  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
-
-  const planning_scene::PlanningSceneConstPtr& the_scene =
-      (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ?
-          static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
-          lscene->diff(goal->planning_options.planning_scene_diff);
-
   rclcpp::Time planning_start = context_->moveit_cpp_->getNode()->now();
   RobotTrajCont traj_vec;
   try
@@ -227,7 +218,8 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(
       return;
     }
 
-    traj_vec = command_list_manager_->solve(the_scene, planning_pipeline, goal->request);
+    auto scene = context_->planning_scene_monitor_->copyPlanningScene(goal->planning_options.planning_scene_diff);
+    traj_vec = command_list_manager_->solve(scene, planning_pipeline, goal->request);
   }
   catch (const MoveItErrorCodeException& ex)
   {
@@ -271,7 +263,6 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::M
 {
   setMoveState(move_group::PLANNING);
 
-  planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
   RobotTrajCont traj_vec;
   try
   {
@@ -285,7 +276,7 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::M
       return false;
     }
 
-    traj_vec = command_list_manager_->solve(plan.planning_scene_, planning_pipeline, req);
+    traj_vec = command_list_manager_->solve(plan.copyPlanningScene(), planning_pipeline, req);
   }
   catch (const MoveItErrorCodeException& ex)
   {

moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp

@@ -77,10 +77,6 @@ bool MoveGroupSequenceService::plan(const moveit_msgs::srv::GetMotionSequence::R
     return true;
   }
 
-  // TODO: Do we lock on the correct scene? Does the lock belong to the scene
-  // used for planning?
-  planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
-
   rclcpp::Time planning_start = context_->moveit_cpp_->getNode()->now();
   RobotTrajCont traj_vec;
   try
@@ -96,7 +92,8 @@ bool MoveGroupSequenceService::plan(const moveit_msgs::srv::GetMotionSequence::R
       return false;
     }
 
-    traj_vec = command_list_manager_->solve(ps, context_->planning_pipeline_, req->request);
+    auto scene = context_->planning_scene_monitor_->copyPlanningScene();
+    traj_vec = command_list_manager_->solve(scene, context_->planning_pipeline_, req->request);
   }
   catch (const MoveItErrorCodeException& ex)
   {

moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

@@ -170,12 +170,6 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGAc
 {
   RCLCPP_INFO(LOGGER, "Planning request received for MoveGroup action. Forwarding to planning pipeline.");
 
-  // lock the scene so that it does not modify the world representation while diff() is called
-  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
-  const planning_scene::PlanningSceneConstPtr& the_scene =
-      (moveit::core::isEmpty(goal->get_goal()->planning_options.planning_scene_diff)) ?
-          static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
-          lscene->diff(goal->get_goal()->planning_options.planning_scene_diff);
   planning_interface::MotionPlanResponse res;
 
   if (preempt_requested_)
@@ -196,7 +190,9 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGAc
 
   try
   {
-    planning_pipeline->generatePlan(the_scene, goal->get_goal()->request, res);
+    auto scene =
+        context_->planning_scene_monitor_->copyPlanningScene(goal->get_goal()->planning_options.planning_scene_diff);
+    planning_pipeline->generatePlan(scene, goal->get_goal()->request, res);
   }
   catch (std::exception& ex)
   {
@@ -225,10 +221,9 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo
     return solved;
   }
 
-  planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
   try
   {
-    solved = planning_pipeline->generatePlan(plan.planning_scene_, req, res);
+    solved = planning_pipeline->generatePlan(plan.copyPlanningScene(), req, res);
   }
   catch (std::exception& ex)
   {

moveit_ros/planning/moveit_cpp/src/planning_component.cpp

@@ -121,10 +121,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
       moveit_cpp_->getPlanningSceneMonitorNonConst();
   planning_scene_monitor->updateFrameTransforms();
-  const planning_scene::PlanningScenePtr planning_scene = [planning_scene_monitor] {
-    planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
-    return planning_scene::PlanningScene::clone(ls);
-  }();
+  const planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor->copyPlanningScene();
   planning_scene_monitor.reset();  // release this pointer
 
   // Init MotionPlanRequest

moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h

@@ -82,6 +82,15 @@ struct ExecutableMotionPlan
 
   /// An error code reflecting what went wrong (if anything)
   moveit_msgs::msg::MoveItErrorCodes error_code_;
+
+  planning_scene::PlanningScenePtr copyPlanningScene()
+  {
+    // planning_scene_ is based on the scene from this monitor
+    // (either it's the monitored scene or a diff on top of it)
+    // so in order to copy the scene, we must also lock the underlying monitor
+    planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor_);
+    return planning_scene::PlanningScene::clone(planning_scene_);
+  }
 };
 
 /// The signature of a function that can compute a motion plan

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -222,6 +222,10 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
     return scene_const_;
   }
 
+  /** @brief Returns a copy of the current planning scene. */
+  planning_scene::PlanningScenePtr
+  copyPlanningScene(const moveit_msgs::msg::PlanningScene& diff = moveit_msgs::msg::PlanningScene());
+
   /** @brief Return true if the scene \e scene can be updated directly
       or indirectly by this monitor. This function will return true if
       the pointer of the scene is the same as the one maintained,

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -152,6 +152,19 @@ PlanningSceneMonitor::~PlanningSceneMonitor()
   rm_loader_.reset();
 }
 
+planning_scene::PlanningScenePtr PlanningSceneMonitor::copyPlanningScene(const moveit_msgs::msg::PlanningScene& diff)
+{
+  // We cannot use LockedPlanningSceneRO for RAII because it requires a PSMPtr
+  // Instead assume clone will not throw
+  lockSceneRead();
+  auto scene = planning_scene::PlanningScene::clone(getPlanningScene());
+  unlockSceneRead();
+
+  if (!moveit::core::isEmpty(diff))
+    scene->setPlanningSceneDiffMsg(diff);
+  return scene;
+}
+
 void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
 {
   if (monitor_name_.empty())