Fixup for #1420: Restore constness of generatePlan() (#1699)

moveit · Nov 8, 2022 · bbb9264 · bbb9264
1 parent abbbb9d
commit bbb9264
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 7 deletions.
diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h
@@ -172,7 +172,7 @@ class MoveItCpp
                                                      const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                                                      bool blocking = true);
 
-  /** \brief Utility to terminate all active planning pipelines */
+  /** \brief Utility to terminate the given planning pipeline */
   bool terminatePlanningPipeline(const std::string& pipeline_name);
 
 private:

diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp
@@ -39,6 +39,7 @@
 
 #include <algorithm>
 #include <moveit/planning_interface/planning_response.h>
+#include <mutex>
 #include <thread>
 
 namespace moveit_cpp
@@ -56,7 +57,7 @@ class PlanSolutions
 
   /** \brief Thread safe method to add PlanSolutions to this data structure TODO(sjahr): Refactor this method to an
    * insert method similar to https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161.
-   * This way, it is possible to created a sorted container e.g. according to a user specified criteria
+   * This way, it is possible to create a sorted container e.g. according to a user specified criteria
    */
   void pushBack(planning_interface::MotionPlanResponse plan_solution)
   {

diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
@@ -133,7 +133,8 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
       \param req The request for motion planning
       \param res The motion planning response */
   bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res);
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const;
 
   /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any).
       \param planning_scene The planning scene where motion planning is to be done
@@ -145,7 +146,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
      invalid in all situations. */
   bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
                     const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& adapter_added_state_index);
+                    std::vector<std::size_t>& adapter_added_state_index) const;
 
   /** \brief Request termination, if a generatePlan() function is currently computing plans */
   void terminate() const;
@@ -184,7 +185,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
   void configure();
 
   // Flag that indicates whether or not the planning pipeline is currently solving a planning problem
-  std::atomic<bool> active_;
+  mutable std::atomic<bool> active_;
 
   std::shared_ptr<rclcpp::Node> node_;
   std::string parameter_namespace_;

diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -232,7 +232,7 @@ void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag)
 
 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                        const planning_interface::MotionPlanRequest& req,
-                                                       planning_interface::MotionPlanResponse& res)
+                                                       planning_interface::MotionPlanResponse& res) const
 {
   std::vector<std::size_t> dummy;
   return generatePlan(planning_scene, req, res, dummy);
@@ -241,7 +241,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                        const planning_interface::MotionPlanRequest& req,
                                                        planning_interface::MotionPlanResponse& res,
-                                                       std::vector<std::size_t>& adapter_added_state_index)
+                                                       std::vector<std::size_t>& adapter_added_state_index) const
 {
   // Set planning pipeline active
   active_ = true;