Merge branch 'main' into ibrahim/param_coeff

moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp

@@ -115,7 +115,7 @@ void init_moveit_py(py::module& m)
 
              if (provide_planning_service)
              {
-               moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
+               moveit_cpp_ptr->getPlanningSceneMonitorNonConst()->providePlanningSceneService();
              };
 
              return moveit_cpp_ptr;

moveit_ros/move_group/src/move_group.cpp

@@ -278,7 +278,7 @@ int main(int argc, char** argv)
   // Initialize MoveItCpp
   const auto tf_buffer = std::make_shared<tf2_ros::Buffer>(nh->get_clock(), tf2::durationFromSec(10.0));
   const auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(nh, moveit_cpp_options, tf_buffer);
-  const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitor();
+  const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst();
 
   if (planning_scene_monitor->getPlanningScene())
   {

moveit_ros/move_group/src/move_group_context.cpp

@@ -46,7 +46,7 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m
                                                const std::string& default_planning_pipeline,
                                                bool allow_trajectory_execution, bool debug)
   : moveit_cpp_(moveit_cpp)
-  , planning_scene_monitor_(moveit_cpp->getPlanningSceneMonitor())
+  , planning_scene_monitor_(moveit_cpp->getPlanningSceneMonitorNonConst())
   , allow_trajectory_execution_(allow_trajectory_execution)
   , debug_(debug)
 {
@@ -74,7 +74,7 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m
 
   if (allow_trajectory_execution_)
   {
-    trajectory_execution_manager_ = moveit_cpp_->getTrajectoryExecutionManager();
+    trajectory_execution_manager_ = moveit_cpp_->getTrajectoryExecutionManagerNonConst();
     plan_execution_ = std::make_shared<plan_execution::PlanExecution>(moveit_cpp_->getNode(), planning_scene_monitor_,
                                                                       trajectory_execution_manager_);
   }

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

@@ -153,13 +153,14 @@ class MoveItCpp
   const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;
 
   /** \brief Get the stored instance of the planning scene monitor */
-  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const;
+  planning_scene_monitor::PlanningSceneMonitorConstPtr getPlanningSceneMonitor() const;
   planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst();
 
-  const std::shared_ptr<tf2_ros::Buffer>& getTFBuffer() const;
+  std::shared_ptr<const tf2_ros::Buffer> getTFBuffer() const;
+  std::shared_ptr<tf2_ros::Buffer> getTFBuffer();
 
   /** \brief Get the stored instance of the trajectory execution manager */
-  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const;
+  trajectory_execution_manager::TrajectoryExecutionManagerConstPtr getTrajectoryExecutionManager() const;
   trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst();
 
   /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager.
@@ -185,7 +186,6 @@ class MoveItCpp
 private:
   //  Core properties and instances
   rclcpp::Node::SharedPtr node_;
-  moveit::core::RobotModelConstPtr robot_model_;
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
   // Planning
@@ -195,9 +195,6 @@ class MoveItCpp
   // Execution
   trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
 
-  /** \brief Reset all member variables */
-  void clearContents();
-
   /** \brief Initialize and setup the planning scene monitor */
   bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options);
 

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -60,8 +60,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options
     throw std::runtime_error(error);
   }
 
-  robot_model_ = planning_scene_monitor_->getRobotModel();
-  if (!robot_model_)
+  if (!getRobotModel())
   {
     const std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
                               "parameter server.";
@@ -78,15 +77,14 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options
   }
 
   trajectory_execution_manager_ = std::make_shared<trajectory_execution_manager::TrajectoryExecutionManager>(
-      node_, robot_model_, planning_scene_monitor_->getStateMonitor());
+      node_, getRobotModel(), planning_scene_monitor_->getStateMonitor());
 
   RCLCPP_DEBUG(LOGGER, "MoveItCpp running");
 }
 
 MoveItCpp::~MoveItCpp()
 {
   RCLCPP_INFO(LOGGER, "Deleting MoveItCpp");
-  clearContents();
 }
 
 bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options)
@@ -130,7 +128,8 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
 {
   // TODO(henningkayser): Use parent namespace for planning pipeline config lookup
   planning_pipelines_ =
-      moveit::planning_pipeline_interfaces::createPlanningPipelineMap(options.pipeline_names, robot_model_, node_);
+      moveit::planning_pipeline_interfaces::createPlanningPipelineMap(options.pipeline_names, getRobotModel(), node_);
+
   if (planning_pipelines_.empty())
   {
     RCLCPP_ERROR(LOGGER, "Failed to load any planning pipelines.");
@@ -141,7 +140,7 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
 
 moveit::core::RobotModelConstPtr MoveItCpp::getRobotModel() const
 {
-  return robot_model_;
+  return planning_scene_monitor_->getRobotModel();
 }
 
 const rclcpp::Node::SharedPtr& MoveItCpp::getNode() const
@@ -175,7 +174,7 @@ const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& M
   return planning_pipelines_;
 }
 
-const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const
+planning_scene_monitor::PlanningSceneMonitorConstPtr MoveItCpp::getPlanningSceneMonitor() const
 {
   return planning_scene_monitor_;
 }
@@ -185,7 +184,7 @@ planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonit
   return planning_scene_monitor_;
 }
 
-const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const
+trajectory_execution_manager::TrajectoryExecutionManagerConstPtr MoveItCpp::getTrajectoryExecutionManager() const
 {
   return trajectory_execution_manager_;
 }
@@ -258,15 +257,13 @@ bool MoveItCpp::terminatePlanningPipeline(const std::string& pipeline_name)
   }
 }
 
-const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const
+std::shared_ptr<const tf2_ros::Buffer> MoveItCpp::getTFBuffer() const
 {
   return planning_scene_monitor_->getTFClient();
 }
-
-void MoveItCpp::clearContents()
+std::shared_ptr<tf2_ros::Buffer> MoveItCpp::getTFBuffer()
 {
-  planning_scene_monitor_.reset();
-  robot_model_.reset();
-  planning_pipelines_.clear();
+  return planning_scene_monitor_->getTFClient();
 }
+
 }  // namespace moveit_cpp

moveit_ros/planning/moveit_cpp/src/planning_component.cpp

@@ -126,7 +126,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest
 
   if (!planning_scene)
   {  // Clone current planning scene
-    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor();
+    auto planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst();
     planning_scene_monitor->updateFrameTransforms();
     planning_scene = [planning_scene_monitor] {
       planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
@@ -171,7 +171,8 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(
 
   if (!planning_scene)
   {  // Clone current planning scene
-    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor();
+    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
+        moveit_cpp_->getPlanningSceneMonitorNonConst();
     planning_scene_monitor->updateFrameTransforms();
     planning_scene = [planning_scene_monitor] {
       planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);

moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp

@@ -57,7 +57,7 @@ class MoveItCppTest : public ::testing::Test
   {
     nh_ = ros::NodeHandle();
     moveit_cpp_ptr = std::make_shared<MoveItCpp>(nh_);
-    trajectory_execution_manager_ptr = moveit_cpp_ptr->getTrajectoryExecutionManager();
+    trajectory_execution_manager_ptr = moveit_cpp_ptr->getTrajectoryExecutionManagerNonConst();
 
     traj1.joint_trajectory.joint_names.push_back("panda_joint1");
     traj1.joint_trajectory.points.resize(1);