Ports moveit1 #3689 (#3357)

* Renames .hpp files to .h to help cherry pick.

* Publish planning scene while planning (#3689)

So far, the PSM's planning scene was locked during planning, thus stopping any PS updates in rviz.
Instead of locking the PSM's current scene, we use an independent clone of that scene for planning. To this end, PlanningSceneMonitor::copyPlanningScene() and ExecutableMotionPlan::copyPlanningScene() were added.

* Renames .h files back to .hpp.

* Updates message types to ros2.

* Updates to use new variable names.

* Undoes erroneous autoformatting.

Author: riv-mjohnson

moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp

@@ -209,15 +209,6 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly(
 {
   RCLCPP_INFO(getLogger(), "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
@@ -233,7 +224,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)
   {
@@ -277,7 +269,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
   {
@@ -291,7 +282,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

@@ -84,10 +84,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
@@ -103,7 +99,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

@@ -193,12 +193,6 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGAc
 {
   RCLCPP_INFO(getLogger(), "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_)
@@ -219,7 +213,9 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGAc
 
   try
   {
-    if (!planning_pipeline->generatePlan(the_scene, goal->get_goal()->request, res, context_->debug_))
+    auto scene =
+        context_->planning_scene_monitor_->copyPlanningScene(goal->get_goal()->planning_options.planning_scene_diff);
+    if (!planning_pipeline->generatePlan(scene, goal->get_goal()->request, res, context_->debug_))
     {
       RCLCPP_ERROR(getLogger(), "Generating a plan with planning pipeline failed.");
       res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
@@ -252,10 +248,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, context_->debug_);
+    solved = planning_pipeline->generatePlan(plan.copyPlanningScene(), req, res, context_->debug_);
   }
   catch (std::exception& ex)
   {

moveit_ros/planning/moveit_cpp/src/planning_component.cpp

@@ -131,11 +131,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest
   {  // Clone current planning scene
     auto planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst();
     planning_scene_monitor->updateFrameTransforms();
-    planning_scene = [planning_scene_monitor] {
-      planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
-      return planning_scene::PlanningScene::clone(ls);
-    }();
-    planning_scene_monitor.reset();  // release this pointer}
+    planning_scene = planning_scene_monitor->copyPlanningScene();
   }
   // Init MotionPlanRequest
   ::planning_interface::MotionPlanRequest request = getMotionPlanRequest(parameters);

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

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

@@ -192,6 +192,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())