Merge branch 'main' into pr-moveit-simple-controller-manager-parameters

.github/workflows/tutorial_docker.yaml

@@ -50,7 +50,7 @@ jobs:
           path: .ccache
           key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }}
       - name: inject ccache into docker
-        uses: reproducible-containers/[email protected].2
+        uses: reproducible-containers/[email protected].3
         with:
           cache-source: .ccache
           cache-target: /root/.ccache/

MIGRATION.md

@@ -3,6 +3,13 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
+- [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this:
+```diff
+- planning_plugin: ompl_interface/OMPLPlanner
++ planning_plugins:
++   - ompl_interface/OMPLPlanner
+```
+
 - [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: >-

moveit_configs_utils/default_configs/chomp_planning.yaml

@@ -1,4 +1,5 @@
-planning_plugin: chomp_interface/CHOMPPlanner
+planning_plugins:
+  - chomp_interface/CHOMPPlanner
 enable_failure_recovery: true
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:

moveit_configs_utils/default_configs/ompl_planning.yaml

@@ -1,4 +1,5 @@
-planning_plugin: ompl_interface/OMPLPlanner
+planning_plugins:
+  - ompl_interface/OMPLPlanner
 # 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

moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml

@@ -1,6 +1,14 @@
-planning_plugin: pilz_industrial_motion_planner/CommandPlanner
-# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+planning_plugins:
+  - pilz_industrial_motion_planner/CommandPlanner
 default_planner_config: PTP
+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/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
 capabilities: >-
     pilz_industrial_motion_planner/MoveGroupSequenceAction
     pilz_industrial_motion_planner/MoveGroupSequenceService

moveit_configs_utils/default_configs/stomp_planning.yaml

@@ -1,5 +1,5 @@
-planning_plugin: stomp_moveit/StompPlanner
-# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+planning_plugins:
+  - stomp_moveit/StompPlanner
 request_adapters:
   - default_planning_request_adapters/ResolveConstraintFrames
   - default_planning_request_adapters/ValidateWorkspaceBounds

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -141,41 +141,7 @@ struct CostSource
   }
 };
 
-/** \brief Representation of a collision checking result */
-struct CollisionResult
-{
-  using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  /** \brief Clear a previously stored result */
-  void clear()
-  {
-    collision = false;
-    distance = std::numeric_limits<double>::max();
-    contact_count = 0;
-    contacts.clear();
-    cost_sources.clear();
-  }
-
-  /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */
-  void print() const;
-
-  /** \brief True if collision was found, false otherwise */
-  bool collision = false;
-
-  /** \brief Closest distance between two bodies */
-  double distance = std::numeric_limits<double>::max();
-
-  /** \brief Number of contacts returned */
-  std::size_t contact_count = 0;
-
-  /** \brief A map returning the pairs of body ids in contact, plus their contact details */
-  ContactMap contacts;
-
-  /** \brief These are the individual cost sources when costs are computed */
-  std::set<CostSource> cost_sources;
-};
+struct CollisionResult;
 
 /** \brief Representation of a collision checking request */
 struct CollisionRequest
@@ -187,6 +153,9 @@ struct CollisionRequest
   /** \brief If true, compute proximity distance */
   bool distance = false;
 
+  /** \brief If true, return detailed distance information. Distance must be set to true as well */
+  bool detailed_distance = false;
+
   /** \brief If true, a collision cost is computed */
   bool cost = false;
 
@@ -354,4 +323,43 @@ struct DistanceResult
     distances.clear();
   }
 };
+
+/** \brief Representation of a collision checking result */
+struct CollisionResult
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /** \brief Clear a previously stored result */
+  void clear()
+  {
+    collision = false;
+    distance = std::numeric_limits<double>::max();
+    distance_result.clear();
+    contact_count = 0;
+    contacts.clear();
+    cost_sources.clear();
+  }
+
+  /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */
+  void print() const;
+
+  /** \brief True if collision was found, false otherwise */
+  bool collision = false;
+
+  /** \brief Closest distance between two bodies */
+  double distance = std::numeric_limits<double>::max();
+
+  /** \brief Distance data for each link */
+  DistanceResult distance_result;
+
+  /** \brief Number of contacts returned */
+  std::size_t contact_count = 0;
+
+  /** \brief A map returning the pairs of body ids in contact, plus their contact details */
+  using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
+  ContactMap contacts;
+
+  /** \brief These are the individual cost sources when costs are computed */
+  std::set<CostSource> cost_sources;
+};
 }  // namespace collision_detection

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

@@ -290,6 +290,10 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll
     dreq.enableGroup(getRobotModel());
     distanceSelf(dreq, dres, state);
     res.distance = dres.minimum_distance.distance;
+    if (req.detailed_distance)
+    {
+      res.distance_result = dres;
+    }
   }
 }
 
@@ -343,6 +347,10 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col
     dreq.enableGroup(getRobotModel());
     distanceRobot(dreq, dres, state);
     res.distance = dres.minimum_distance.distance;
+    if (req.detailed_distance)
+    {
+      res.distance_result = dres;
+    }
   }
 }
 

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

@@ -164,8 +164,8 @@ class PlannerManager
   virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node,
                           const std::string& parameter_namespace);
 
-  /// Get \brief a short string that identifies the planning interface
-  virtual std::string getDescription() const;
+  /// \brief Get a short string that identifies the planning interface.
+  virtual std::string getDescription() const = 0;
 
   /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning
   /// request)

moveit_core/utils/CMakeLists.txt

@@ -8,7 +8,8 @@ target_include_directories(moveit_utils PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/moveit_core>
 )
-ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp rsl fmt)
+ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp fmt)
+target_link_libraries(moveit_utils rsl::rsl)
 set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 install(DIRECTORY include/ DESTINATION include/moveit_core)
@@ -20,7 +21,7 @@ target_include_directories(moveit_test_utils PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/moveit_core>
 )
-target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base)
+target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base rsl::rsl)
 ament_target_dependencies(moveit_test_utils
   ament_index_cpp
   Boost
@@ -30,7 +31,6 @@ ament_target_dependencies(moveit_test_utils
   urdfdom
   urdfdom_headers
   rclcpp
-  rsl
   fmt
 )
 set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py

@@ -18,8 +18,8 @@ def generate_test_description():
     # Load the context
     test_config = load_moveit_config()
 
-    planning_plugin = {
-        "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner"
+    planning_plugins = {
+        "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"]
     }
 
     # run test
@@ -29,7 +29,7 @@ def generate_test_description():
         name="unittest_pilz_industrial_motion_planner",
         parameters=[
             test_config.to_dict(),
-            planning_plugin,
+            planning_plugins,
         ],
         output="screen",
     )

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp

@@ -74,8 +74,8 @@ class CommandPlannerTest : public testing::Test
     ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
 
     // Load planner name from node parameters
-    ASSERT_TRUE(node_->has_parameter("planning_plugin")) << "Could not find parameter 'planning_plugin'";
-    node_->get_parameter<std::string>("planning_plugin", planner_plugin_name_);
+    ASSERT_TRUE(node_->has_parameter("planning_plugins")) << "Could not find parameter 'planning_plugins'";
+    node_->get_parameter<std::vector<std::string>>("planning_plugins", planner_plugin_names_);
 
     // Load the plugin
     try
@@ -92,7 +92,7 @@ class CommandPlannerTest : public testing::Test
     // Create planner
     try
     {
-      planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_));
+      planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_names_.at(0)));
       ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, ""))
           << "Initializing the planner instance failed.";
     }
@@ -104,7 +104,7 @@ class CommandPlannerTest : public testing::Test
 
   void TearDown() override
   {
-    planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_);
+    planner_plugin_loader_->unloadLibraryForClass(planner_plugin_names_.at(0));
     robot_model_.reset();
   }
 
@@ -114,7 +114,7 @@ class CommandPlannerTest : public testing::Test
   moveit::core::RobotModelConstPtr robot_model_;
   std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
 
-  std::string planner_plugin_name_;
+  std::vector<std::string> planner_plugin_names_;
   std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader_;
   planning_interface::PlannerManagerPtr planner_instance_;
 };

moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp

@@ -118,12 +118,6 @@ class StompPlannerManager : public PlannerManager
       RCLCPP_ERROR(LOGGER, "Invalid joint group '%s'", req.group_name.c_str());
       return false;
     }
-
-    if (!req.reference_trajectories.empty())
-    {
-      RCLCPP_WARN(LOGGER, "Ignoring reference trajectories - not implemented!");
-    }
-
     return true;
   }
 

moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py

@@ -81,12 +81,20 @@ def generate_launch_description():
         "default_planning_pipeline": "ompl",
         "planning_pipelines": ["pilz", "ompl"],
         "pilz": {
-            "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner",
-            "request_adapters": "",
+            "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"],
         },
         "ompl": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""",
+            "planning_plugins": ["ompl_interface/OMPLPlanner"],
+            "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",
+            ],
         },
     }
     ompl_planning_yaml = load_yaml(

moveit_py/moveit/policies/__init__.py

@@ -0,0 +1 @@
+from .policy import Policy