Add pipeline state publisher

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

@@ -47,6 +47,7 @@
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <moveit_msgs/msg/display_trajectory.hpp>
+#include <moveit_msgs/msg/pipeline_state.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 #include <memory>
 #include <moveit_planning_pipeline_export.h>
@@ -107,9 +108,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
   /** \brief When motion plans are computed and they are supposed to be automatically displayed, they are sent to this
    * topic (moveit_msgs::msg::DisplauTrajectory) */
   static inline const std::string DISPLAY_PATH_TOPIC = std::string("display_planned_path");
-  /** \brief When motion planning requests are received and they are supposed to be automatically published, they are
-   * sent to this topic (moveit_msgs::msg::MotionPlanRequest) */
-  static inline const std::string MOTION_PLAN_REQUEST_TOPIC = std::string("motion_plan_request");
   /** \brief When contacts are found in the solution path reported by a planner, they can be published as markers on
    * this topic (visualization_msgs::MarkerArray) */
   static inline const std::string MOTION_CONTACTS_TOPIC = std::string("display_contacts");
@@ -229,6 +227,16 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
   /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance
   void configure();
 
+  /**
+   * @brief Helper function to publish the planning pipeline state during the planning process
+   *
+   * @param req Current request to publish
+   * @param res Current pipeline result
+   * @param pipeline_stage Last pipeline stage that got invoked
+   */
+  void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, const planning_interface::MotionPlanResponse& res,
+                            const std::string& pipeline_stage) const;
+
   // Flag that indicates whether or not the planning pipeline is currently solving a planning problem
   mutable std::atomic<bool> active_;
 
@@ -240,9 +248,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
   /// Optionally publish motion plans as a moveit_msgs::msg::DisplayTrajectory
   rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_path_publisher_;
 
-  /// Optionally publish the request before beginning processing (useful for debugging)
-  rclcpp::Publisher<moveit_msgs::msg::MotionPlanRequest>::SharedPtr received_request_publisher_;
-
   // Planner plugin
   std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader_;
   planning_interface::PlannerManagerPtr planner_instance_;
@@ -261,6 +266,9 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
   /// Publish contacts if the generated plans are checked again by the planning pipeline
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr contacts_publisher_;
 
+  /// Publish the planning pipeline progress
+  rclcpp::Publisher<moveit_msgs::msg::PipelineState>::SharedPtr progress_publisher_;
+
   rclcpp::Logger logger_;
 };
 

moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml

@@ -17,3 +17,8 @@ planning_pipeline_parameters:
     read_only: true,
     default_value: [],
   }
+  progress_topic: {
+    type: string,
+    description: "For every stage of the planning pipeline a progress message is published to this topic. An empty string disables the publisher.",
+    default_value: "pipeline_state",
+  }

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

@@ -85,13 +85,18 @@ void PlanningPipeline::configure()
   }
 
   // Initialize optional publishers for debugging
-  received_request_publisher_ = node_->create_publisher<moveit_msgs::msg::MotionPlanRequest>(
-      MOTION_PLAN_REQUEST_TOPIC, rclcpp::SystemDefaultsQoS());
   contacts_publisher_ =
       node_->create_publisher<visualization_msgs::msg::MarkerArray>(MOTION_CONTACTS_TOPIC, rclcpp::SystemDefaultsQoS());
   display_path_publisher_ =
       node_->create_publisher<moveit_msgs::msg::DisplayTrajectory>(DISPLAY_PATH_TOPIC, rclcpp::SystemDefaultsQoS());
 
+  // If progress topic parameter is not empty, initialize publisher
+  if (!pipeline_parameters_.progress_topic.empty())
+  {
+    progress_publisher_ = node_->create_publisher<moveit_msgs::msg::PipelineState>(pipeline_parameters_.progress_topic,
+                                                                                   rclcpp::SystemDefaultsQoS());
+  }
+
   // Create planner plugin loader
   try
   {
@@ -189,6 +194,20 @@ void PlanningPipeline::configure()
   }
 }
 
+void PlanningPipeline::publishPipelineState(moveit_msgs::msg::MotionPlanRequest req,
+                                            const planning_interface::MotionPlanResponse& res,
+                                            const std::string& pipeline_stage) const
+{
+  if (progress_publisher_)
+  {
+    moveit_msgs::msg::PipelineState progress;
+    progress.request = req;
+    res.getMessage(progress.response);
+    progress.pipeline_stage = pipeline_stage;
+    progress_publisher_->publish(progress);
+  }
+}
+
 bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                     const planning_interface::MotionPlanRequest& req,
                                     planning_interface::MotionPlanResponse& res, const bool publish_received_requests,
@@ -202,7 +221,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
   // broadcast the request we are about to work on, if needed
   if (publish_received_requests)
   {
-    received_request_publisher_->publish(req);
+    publishPipelineState(req, res, std::string(""));
   }
 
   // ---------------------------------
@@ -219,6 +238,8 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
       assert(req_adapter);
       RCLCPP_INFO(node_->get_logger(), "Calling PlanningRequestAdapter '%s'", req_adapter->getDescription().c_str());
       solved = req_adapter->adapt(planning_scene, mutable_request);
+      // Publish progress
+      publishPipelineState(mutable_request, res, req_adapter->getDescription());
       // If adapter does not succeed, break chain and return false
       if (!solved)
       {
@@ -233,6 +254,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
       planning_interface::PlanningContextPtr context =
           planner_instance_->getPlanningContext(planning_scene, mutable_request, res.error_code);
       solved = context ? context->solve(res) : false;
+      publishPipelineState(mutable_request, res, planner_instance_->getDescription());
     }
 
     // Call plan response adapter chain
@@ -244,6 +266,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
         assert(res_adapter);
         RCLCPP_INFO(node_->get_logger(), "Calling PlanningResponseAdapter '%s'", res_adapter->getDescription().c_str());
         solved = res_adapter->adapt(planning_scene, mutable_request, res);
+        publishPipelineState(mutable_request, res, res_adapter->getDescription());
         // If adapter does not succeed, break chain and return false
         if (!solved)
         {