Refactor planning pipeline chain

Move plan_request_adapters into planning interface package

Add callRequestAdapterChain function

Differentiate between request and response adapters

Restore planning_pipeline.cpp

Add response_adapters parameter and cleanup adapter chain calls

Add response adapter loading chain

Address compiler errors

Fix parameter type

Fix compilation errors

Rename planning_request_adapters to *_adapter

Cleanup + fix errors

Fix plugins and print description

Remove unused adapters

Planners plan, Adapters adapt ...

Format + fix compilation error

Simplify by removing callRequest/ResponseAdapterChain

Remove unneeded PlannerFn

Make clang tidy happy

Add loadPluginVector template and make callAdapterChain private

Add documentation

Remove TODOs

Cleanups and move templates into annonymous namespace

Add more debug information and fix bugs in adapters

Fix bugs

Remove commented out CMake lines

Update parameter description

Apply suggestions from code review

Co-authored-by: Abishalini Sivaraman <abi.gpuram@gmail.com>

Address review

Remove instrospection dir

Make parameters read only

Make sure that pipeline does not abort if no request adapter is configured

Remove anonymous namespace in header

Move adapter params into separate namespace

Delete outdated unittest

Apply suggestions from code review

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

ruckig -> Ruckig!

Address comments part 2

Remove wrong request_adapters initilization from PILZ config

Update migration guide

Update logging

Remove response argument from PlanningRequestAdapter::adapt

Make adapter naming consistent

Add pipeline state publisher

Pull stacked constraints check out of planning pipeline

Update planning request adapters

Format!

Remove unused params and fix clang tidy

Fix error

Move validity check and path publisher in separate adapters

Fix DISPLAY_PATH_TOPIC

Update config yamls

Add MoveItStatus and return with request adapter

Revert outdated change

Request adapters adapt returns void now

Planner solve function returns void now

Update license in header

Apply suggestions from code review

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Update migration guide

Address review

Don't make initialize purely virtual & address review

MIGRATION.md

@@ -3,19 +3,39 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
-- `lockSceneRead()` and `lockSceneWrite()` are now protected member functions, for internal use only. To lock the planning scene, use LockedPlanningSceneRO or LockedPlanningSceneRW:
+- [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this:
+```diff
+- request_adapters: >-
+-     default_planner_request_adapters/AddTimeOptimalParameterization
+-     default_planner_request_adapters/ResolveConstraintFrames
+-     default_planner_request_adapters/FixWorkspaceBounds
+-     default_planner_request_adapters/FixStartStateBounds
+-     default_planner_request_adapters/FixStartStateCollision
+-     default_planner_request_adapters/FixStartStatePathConstraints
++ # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
++ request_adapters:
++   - default_planning_request_adapters/ResolveConstraintFrames
++   - default_planning_request_adapters/ValidateWorkspaceBounds
++   - default_planning_request_adapters/CheckStartStateBounds
++   - default_planning_request_adapters/CheckStartStateCollision
++ response_adapters:
++   - default_planning_response_adapters/AddTimeOptimalParameterization
++   - default_planning_response_adapters/ValidateSolution
++   - default_planning_response_adapters/DisplayMotionPath
+```
+- [2021] `lockSceneRead()` and `lockSceneWrite()` are now protected member functions, for internal use only. To lock the planning scene, use ``LockedPlanningSceneRO`` or ``LockedPlanningSceneRW``:
 ```
       planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
       moveit::core::RobotModelConstPtr model = ls->getRobotModel();
 ```
-- ServoServer was renamed to ServoNode
-- `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes:
+- [2021] ServoServer was renamed to ServoNode
+- [2021] `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes:
     - `getFrameTransform()` now returns this pose instead of the first shape's pose.
     - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest.
     - Planning scene geometry text files (`.scene`) have changed format. Loading old files is still supported. You can add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files.
-- add API for passing RNG to setToRandomPositionsNearBy
-- Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
-- Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic).
+- [2021] Add API for passing RNG to setToRandomPositionsNearBy
+- [2021] Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
+- [2021] Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic).
 
 ## ROS Noetic
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.

moveit_configs_utils/default_configs/chomp_planning.yaml

@@ -1,12 +1,14 @@
 planning_plugin: chomp_interface/CHOMPPlanner
 enable_failure_recovery: true
-jiggle_fraction: 0.05
-request_adapters: >-
-    default_planner_request_adapters/AddTimeOptimalParameterization
-    default_planner_request_adapters/ResolveConstraintFrames
-    default_planner_request_adapters/FixWorkspaceBounds
-    default_planner_request_adapters/FixStartStateBounds
-    default_planner_request_adapters/FixStartStateCollision
-    default_planner_request_adapters/FixStartStatePathConstraints
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+
 ridge_factor: 0.01
-start_state_max_bounds_error: 0.1

moveit_configs_utils/default_configs/ompl_planning.yaml

@@ -1,10 +1,11 @@
 planning_plugin: ompl_interface/OMPLPlanner
-start_state_max_bounds_error: 0.1
-jiggle_fraction: 0.05
-request_adapters: >-
-    default_planner_request_adapters/AddTimeOptimalParameterization
-    default_planner_request_adapters/ResolveConstraintFrames
-    default_planner_request_adapters/FixWorkspaceBounds
-    default_planner_request_adapters/FixStartStateBounds
-    default_planner_request_adapters/FixStartStateCollision
-    default_planner_request_adapters/FixStartStatePathConstraints
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath

moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml

@@ -1,5 +1,5 @@
 planning_plugin: pilz_industrial_motion_planner/CommandPlanner
-request_adapters: ""
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 default_planner_config: PTP
 capabilities: >-
     pilz_industrial_motion_planner/MoveGroupSequenceAction

moveit_configs_utils/default_configs/stomp_planning.yaml

@@ -1,11 +1,14 @@
 planning_plugin: stomp_moveit/StompPlanner
-request_adapters: >-
-  default_planner_request_adapters/AddTimeOptimalParameterization
-  default_planner_request_adapters/ResolveConstraintFrames
-  default_planner_request_adapters/FixWorkspaceBounds
-  default_planner_request_adapters/FixStartStateBounds
-  default_planner_request_adapters/FixStartStateCollision
-  default_planner_request_adapters/FixStartStatePathConstraints
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
 
 stomp_moveit:
   num_timesteps: 60

moveit_core/CMakeLists.txt

@@ -59,7 +59,6 @@ add_subdirectory(kinematics_metrics)
 add_subdirectory(macros)
 add_subdirectory(online_signal_smoothing)
 add_subdirectory(planning_interface)
-add_subdirectory(planning_request_adapter)
 add_subdirectory(planning_scene)
 add_subdirectory(robot_model)
 add_subdirectory(robot_state)
@@ -115,7 +114,6 @@ install(
     moveit_kinematics_metrics
     moveit_macros
     moveit_planning_interface
-    moveit_planning_request_adapter
     moveit_planning_scene
     moveit_robot_model
     moveit_robot_state

moveit_core/planning_interface/CMakeLists.txt

@@ -16,6 +16,7 @@ ament_target_dependencies(moveit_planning_interface
 target_link_libraries(moveit_planning_interface
   moveit_robot_trajectory
   moveit_robot_state
+  moveit_planning_scene
   moveit_utils
 )
 

moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h

@@ -39,16 +39,12 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit/planning_interface/planning_request.h>
 #include <moveit/planning_interface/planning_response.h>
+#include <moveit/planning_scene/planning_scene.h>
 #include <rclcpp/node.hpp>
 #include <string>
 #include <map>
 #include <rclcpp/rclcpp.hpp>
 
-namespace planning_scene
-{
-MOVEIT_CLASS_FORWARD(PlanningScene);  // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc
-}
-
 /** \brief This namespace includes the base class for MoveIt planners */
 namespace planning_interface
 {
@@ -119,11 +115,11 @@ class PlanningContext
 
   /** \brief Solve the motion planning problem and store the result in \e res. This function should not clear data
    * structures before computing. The constructor and clear() do that. */
-  virtual bool solve(MotionPlanResponse& res) = 0;
+  virtual void solve(MotionPlanResponse& res) = 0;
 
   /** \brief Solve the motion planning problem and store the detailed result in \e res. This function should not clear
    * data structures before computing. The constructor and clear() do that. */
-  virtual bool solve(MotionPlanDetailedResponse& res) = 0;
+  virtual void solve(MotionPlanDetailedResponse& res) = 0;
 
   /** \brief If solve() is running, terminate the computation. Return false if termination not possible. No-op if
    * solve() is not running (returns true).*/

moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h

@@ -40,8 +40,8 @@
 
 namespace planning_interface
 {
-// for now this is just a typedef
 
+// for now this is just a typedef
 typedef moveit_msgs::msg::MotionPlanRequest MotionPlanRequest;
 
 }  // namespace planning_interface

moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Ioan Sucan, Sebastian Jahr
+   Description: Generic interface to adapting motion planning requests
+*/
+
+#pragma once
+
+#include <moveit/macros/class_forward.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/utils/moveit_status.h>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/node.hpp>
+
+namespace planning_interface
+{
+MOVEIT_CLASS_FORWARD(PlanningRequestAdapter);  // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc
+
+/** @brief Concept in MoveIt which can be used to modify the planning problem(pre-processing) in a planning pipeline.
+ * PlanningRequestAdapters enable adjusting to or validating a planning problem for a subsequent planning algorithm.
+ */
+class PlanningRequestAdapter
+{
+public:
+  /** @brief Initialize parameters using the passed Node and parameter namespace.
+   *  @param node Node instance used by the adapter
+   *  @param parameter_namespace Parameter namespace for adapter
+      @details If no initialization is needed, simply implement as empty */
+  virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){};
+
+  /** @brief Get a description of this adapter
+   *  @return Returns a short string that identifies the planning request adapter
+   */
+  [[nodiscard]] virtual std::string getDescription() const = 0;
+
+  /** @brief Adapt the planning request
+   *  @param planning_scene Representation of the environment for the planning
+   *  @param req Motion planning request with a set of constraints
+   *  @return True if response was generated correctly */
+  [[nodiscard]] virtual moveit::core::MoveItStatus adapt(const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                                         planning_interface::MotionPlanRequest& req) const = 0;
+};
+}  // namespace planning_interface

moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h

@@ -65,11 +65,6 @@ struct MotionPlanResponse
   moveit_msgs::msg::RobotState start_state;
   std::string planner_id;
 
-  /// Sometimes planning request adapters may add states on the solution path (e.g., add the current state of the robot
-  /// as prefix, when the robot started to plan only from near that state, as the current state itself appears to touch
-  /// obstacles). This is helpful because the added states should not be considered invalid in all situations.
-  std::vector<std::size_t> added_path_index;  // This won't be included into the MotionPlanResponse ROS 2 message!
-
   // \brief Enable checking of query success or failure, for example if(response) ...
   explicit operator bool() const
   {

moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h

@@ -0,0 +1,74 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr */
+
+#pragma once
+
+#include <moveit/macros/class_forward.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/node.hpp>
+
+namespace planning_interface
+{
+MOVEIT_CLASS_FORWARD(PlanningResponseAdapter);  // Defines PlanningResponseAdapterPtr, ConstPtr, WeakPtr... etc
+
+/** @brief Concept in MoveIt which can be used to modify the planning solution (post-processing) in a planning pipeline.
+ * PlanningResponseAdapters enable using for example smoothing and trajectory generation algorithms in sequence to
+ * produce a trajectory.
+ */
+class PlanningResponseAdapter
+{
+public:
+  /** @brief Initialize parameters using the passed Node and parameter namespace.
+   *  @param node Node instance used by the adapter
+   *  @param parameter_namespace Parameter namespace for adapter
+      @details If no initialization is needed, simply implement as empty */
+  virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){};
+
+  /** \brief Get a description of this adapter
+   *  @return Returns a short string that identifies the planning response adapter
+   */
+  [[nodiscard]] virtual std::string getDescription() const = 0;
+
+  /** @brief Adapt the planning response
+   *  @param planning_scene Representation of the environment for the planning
+   *  @param req Motion planning request with a set of constraints
+   *  @param res Motion planning response containing the solution that is adapted. */
+  virtual void adapt(const planning_scene::PlanningSceneConstPtr& planning_scene,
+                     const planning_interface::MotionPlanRequest& req,
+                     planning_interface::MotionPlanResponse& res) const = 0;
+};
+}  // namespace planning_interface