Sync Iron with Main for release

.github/mergify.yml

@@ -26,6 +26,15 @@ pull_request_rules:
         branches:
           - humble
 
+  - name: backport to iron at reviewers discretion
+    conditions:
+      - base=main
+      - "label=backport-iron"
+    actions:
+      backport:
+        branches:
+          - iron
+
   - name: ask to resolve conflict
     conditions:
       - conflict

.github/workflows/ci.yaml

@@ -23,6 +23,10 @@ jobs:
             ROS_DISTRO: iron
             IKFAST_TEST: true
             CLANG_TIDY: pedantic
+          - IMAGE: humble-ci
+            ROS_DISTRO: humble
+          - IMAGE: humble-ci-testing
+            ROS_DISTRO: humble
     env:
       CXXFLAGS: >-
         -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
@@ -77,7 +81,7 @@ jobs:
           # free up a lot of stuff from /usr/local
           sudo rm -rf /usr/local
           df -h
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - uses: testspace-com/setup-testspace@v1
         if: github.repository == 'ros-planning/moveit2'
         with:

.github/workflows/docker.yaml

@@ -154,7 +154,7 @@ jobs:
       PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }}
 
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v2
       - name: Login to Github Container Registry

.github/workflows/docker_lint.yaml

@@ -21,7 +21,7 @@ jobs:
     name: Lint Dockerfiles
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - uses: hadolint/[email protected]
         with:
           dockerfile: .docker/${{ matrix.DOCKERFILE_PATH }}/Dockerfile

.github/workflows/format.yaml

@@ -15,7 +15,7 @@ jobs:
     name: Format
     runs-on: ubuntu-22.04
     steps:
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
     - uses: actions/setup-python@v4
       with:
         python-version: '3.10'

.github/workflows/prerelease.yaml

@@ -21,6 +21,6 @@ jobs:
     name: "${{ matrix.distro }}"
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: industrial_ci
         uses: ros-industrial/industrial_ci@master

.github/workflows/sonar.yaml

@@ -55,7 +55,7 @@ jobs:
           # free up a lot of stuff from /usr/local
           sudo rm -rf /usr/local
           df -h
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - uses: testspace-com/setup-testspace@v1
         if: github.repository == 'ros-planning/moveit2'
         with:

moveit_core/package.xml

@@ -36,6 +36,7 @@
   <depend>generate_parameter_library</depend>
   <depend>geometric_shapes</depend>
   <depend>geometry_msgs</depend>
+  <depend>google_benchmark_vendor</depend>
   <depend>kdl_parser</depend>
   <depend>libfcl-dev</depend>
   <depend>moveit_common</depend>
@@ -67,6 +68,7 @@
   <test_depend>moveit_resources_pr2_description</test_depend>
   <test_depend>angles</test_depend>
   <test_depend>orocos_kdl_vendor</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_index_cpp</test_depend>

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

@@ -65,6 +65,11 @@ 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_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h

@@ -60,8 +60,6 @@ class PlanningRequestAdapter
       std::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
                          planning_interface::MotionPlanResponse&)>;
 
-  virtual ~PlanningRequestAdapter() = default;
-
   /** \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
@@ -71,23 +69,7 @@ class PlanningRequestAdapter
   /** \brief Get a description of this adapter
    *  @return Returns a short string that identifies the planning request adapter
    */
-  virtual std::string getDescription() const
-  {
-    return "";
-  }
-
-  /** \brief Adapt the planning request if needed, call the planner
-      function \e planner and update the planning response if
-      needed.
-   *  @param planner Pointer to the planner used to solve the passed problem
-   *  @param planning_scene Representation of the environment for the planning
-   *  @param req Motion planning request with a set of constraints
-   *  @param res Reference to which the generated motion plan response is written to
-   *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req,
-                    planning_interface::MotionPlanResponse& res) const;
+  [[nodiscard]] virtual std::string getDescription() const = 0;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -98,15 +80,11 @@ class PlanningRequestAdapter
    *  @param planning_scene Representation of the environment for the planning
    *  @param req Motion planning request with a set of constraints
    *  @param res Reference to which the generated motion plan response is written to
-   *  @param added_path_index 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.
    *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& added_path_index) const;
+  [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
+                                  const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                  const planning_interface::MotionPlanRequest& req,
+                                  planning_interface::MotionPlanResponse& res) const;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -117,36 +95,11 @@ class PlanningRequestAdapter
    *  @param planning_scene Representation of the environment for the planning
    *  @param req Motion planning request with a set of constraints
    *  @param res Reference to which the generated motion plan response is written to
-   *  @param added_path_index 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.
    *  @return True if response got generated correctly */
-  virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-                            const planning_interface::MotionPlanRequest& req,
-                            planning_interface::MotionPlanResponse& res,
-                            std::vector<std::size_t>& added_path_index) const = 0;
-
-protected:
-  /** \brief Helper param for getting a parameter using a namespace **/
-  template <typename T>
-  T getParam(const rclcpp::Node::SharedPtr& node, const rclcpp::Logger& logger, const std::string& parameter_namespace,
-             const std::string& parameter_name, T default_value = {}) const
-  {
-    std::string full_name = parameter_namespace.empty() ? parameter_name : parameter_namespace + "." + parameter_name;
-    T value;
-    if (!node->get_parameter(full_name, value))
-    {
-      RCLCPP_INFO(logger, "Param '%s' was not set. Using default value: %s", full_name.c_str(),
-                  std::to_string(default_value).c_str());
-      return default_value;
-    }
-    else
-    {
-      RCLCPP_INFO(logger, "Param '%s' was set to %s", full_name.c_str(), std::to_string(value).c_str());
-      return value;
-    }
-  }
+  [[nodiscard]] virtual bool adaptAndPlan(const PlannerFn& planner,
+                                          const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                          const planning_interface::MotionPlanRequest& req,
+                                          planning_interface::MotionPlanResponse& res) const = 0;
 };
 
 /** \brief Apply a sequence of adapters to a motion plan */
@@ -163,25 +116,10 @@ class PlanningRequestAdapterChain
    *  @param req Motion planning request with a set of constraints
    *  @param res Reference to which the generated motion plan response is written to
    *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req,
-                    planning_interface::MotionPlanResponse& res) const;
-
-  /** \brief Iterate through the chain and call all adapters and planners in the correct order
-   *  @param planner Pointer to the planner used to solve the passed problem
-   *  @param planning_scene Representation of the environment for the planning
-   *  @param req Motion planning request with a set of constraints
-   *  @param res Reference to which the generated motion plan response is written to
-   *  @param added_path_index 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.
-   *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& added_path_index) const;
+  [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
+                                  const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                  const planning_interface::MotionPlanRequest& req,
+                                  planning_interface::MotionPlanResponse& res) const;
 
 private:
   std::vector<PlanningRequestAdapterConstPtr> adapters_;

moveit_core/planning_request_adapter/src/planning_request_adapter.cpp

@@ -64,20 +64,18 @@ bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner
 
 bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner,
                  const planning_scene::PlanningSceneConstPtr& planning_scene,
-                 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                 std::vector<std::size_t>& added_path_index)
+                 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res)
 {
   try
   {
-    bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index);
+    bool result = adapter.adaptAndPlan(planner, planning_scene, req, res);
     RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code));
     return result;
   }
   catch (std::exception& ex)
   {
     RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\nSkipping adapter instead.",
                  adapter.getDescription().c_str(), ex.what());
-    added_path_index.clear();
     return planner(planning_scene, req, res);
   }
 }
@@ -87,24 +85,14 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda
 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                           const planning_scene::PlanningSceneConstPtr& planning_scene,
                                           const planning_interface::MotionPlanRequest& req,
-                                          planning_interface::MotionPlanResponse& res,
-                                          std::vector<std::size_t>& added_path_index) const
+                                          planning_interface::MotionPlanResponse& res) const
 {
   return adaptAndPlan(
       [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
                  planning_interface::MotionPlanResponse& res) {
         return callPlannerInterfaceSolve(*planner, scene, req, res);
       },
-      planning_scene, req, res, added_path_index);
-}
-
-bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                                          const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                          const planning_interface::MotionPlanRequest& req,
-                                          planning_interface::MotionPlanResponse& res) const
-{
-  std::vector<std::size_t> empty_added_path_index;
-  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
+      planning_scene, req, res);
 }
 
 void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter)
@@ -116,25 +104,12 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                const planning_interface::MotionPlanRequest& req,
                                                planning_interface::MotionPlanResponse& res) const
-{
-  std::vector<std::size_t> empty_added_path_index;
-  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
-}
-
-bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                                               const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                               const planning_interface::MotionPlanRequest& req,
-                                               planning_interface::MotionPlanResponse& res,
-                                               std::vector<std::size_t>& added_path_index) const
 {
   // if there are no adapters, run the planner directly
   if (adapters_.empty())
   {
-    added_path_index.clear();
     return callPlannerInterfaceSolve(*planner, planning_scene, req, res);
   }
-  // the index values added by each adapter
-  std::vector<std::vector<std::size_t>> added_path_index_each(adapters_.size());
 
   // if there are adapters, construct a function for each, in order,
   // so that in the end we have a nested sequence of functions that calls all adapters
@@ -147,30 +122,14 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
 
   for (int i = adapters_.size() - 1; i >= 0; --i)
   {
-    fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]](
-             const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-             planning_interface::MotionPlanResponse& res) {
-      return callAdapter(adapter, fn, scene, req, res, added_path_index);
-    };
+    fn = [&adapter = *adapters_[i],
+          fn](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+              planning_interface::MotionPlanResponse& res) { return callAdapter(adapter, fn, scene, req, res); };
   }
 
   bool result = fn(planning_scene, req, res);
-  added_path_index.clear();
 
-  // merge the index values from each adapter
-  for (std::vector<std::size_t>& added_states_by_each_adapter : added_path_index_each)
-  {
-    for (std::size_t& added_index : added_states_by_each_adapter)
-    {
-      for (std::size_t& index_in_path : added_path_index)
-      {
-        if (added_index <= index_in_path)
-          index_in_path++;
-      }
-      added_path_index.push_back(added_index);
-    }
-  }
-  std::sort(added_path_index.begin(), added_path_index.end());
+  std::sort(res.added_path_index.begin(), res.added_path_index.end());
   return result;
 }
 

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -255,7 +255,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
     return world_const_;
   }
 
-  // brief Get the representation of the world
+  /** \brief Get the representation of the world */
   const collision_detection::WorldPtr& getWorldNonConst()
   {
     // we always have a world representation

moveit_core/robot_model/src/robot_model.cpp

@@ -412,7 +412,7 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
           }
           else
           {
-            RCLCPP_ERROR(LOGGER, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
+            RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic joint '%s' because they have different number of DOF",
                          joint_model->getName().c_str(), jm->mimic->joint_name.c_str());
           }
         }

moveit_core/robot_state/CMakeLists.txt

@@ -26,7 +26,9 @@ install(DIRECTORY include/ DESTINATION include/moveit_core)
 
 # Unit tests
 if(BUILD_TESTING)
+  find_package(ament_cmake_google_benchmark REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(benchmark REQUIRED)
   find_package(tf2_geometry_msgs REQUIRED)
   find_package(resource_retriever REQUIRED)
 
@@ -77,4 +79,13 @@ if(BUILD_TESTING)
     moveit_robot_state
     moveit_kinematics_base
   )
+
+  ament_add_google_benchmark(
+    robot_state_jacobian_benchmark
+    test/robot_state_jacobian_benchmark.cpp)
+  target_link_libraries(robot_state_jacobian_benchmark
+    moveit_robot_model
+    moveit_test_utils
+    moveit_robot_state
+  )
 endif()