Merge branch 'moveit:main' into patch-3

Author: mosfet80

.github/workflows/ci.yaml

@@ -34,7 +34,7 @@ jobs:
       # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option
       # https://stackoverflow.com/a/41673702
       CXXFLAGS: >-
-        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option
+        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option -Wno-cpp
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: >
@@ -105,36 +105,36 @@ jobs:
         with:
           file: moveit2.repos
       - name: Cache upstream workspace
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/upstream_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }}
       # The target directory cache doesn't include the source directory because
       # that comes from the checkout.  See "prepare target_ws for cache" task below
       - name: Cache target workspace
         if: "!matrix.env.CCOV"
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/target_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }}
       - name: Cache ccache
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
           restore-keys: |
             ${{ env.CACHE_PREFIX }}-${{ github.sha }}
             ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }}
       - name: Configure ccache
         run: |

.github/workflows/format.yaml

@@ -19,7 +19,7 @@ jobs:
     - uses: actions/checkout@v4
     - uses: actions/setup-python@v5
       with:
-        python-version: '3.10'
+        python-version: 3.x
     - name: Install clang-format-14
       run: sudo apt-get install clang-format-14
     - uses: pre-commit/[email protected]

.github/workflows/sonar.yaml

@@ -72,23 +72,24 @@ jobs:
         with:
           file: moveit2.repos
       - name: Cache upstream workspace
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/upstream_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-rolling-ci-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }}
       - name: Cache ccache
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-rolling-ci-ccov-${{ github.sha }}-${{ github.run_id }}
           restore-keys: |
             ccache-rolling-ci-ccov-${{ github.sha }}
             ccache-rolling-ci-ccov
-          save-always: true
+        env:
+          GHA_CACHE_SAVE: always
       - name: Configure ccache
         run: |
           mkdir -p ${{ env.CCACHE_DIR }}

moveit2.repos

@@ -12,3 +12,7 @@ repositories:
     type: git
     url: https://github.com/moveit/moveit_resources.git
     version: ros2
+  srdfdom:
+    type: git
+    url: https://github.com/moveit/srdfdom.git
+    version: ros2

moveit_configs_utils/moveit_configs_utils/launches.py

@@ -204,7 +204,12 @@ def generate_move_group_launch(moveit_config):
         )
     )
     # inhibit these default MoveGroup capabilities (space separated)
-    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
+    ld.add_action(
+        DeclareLaunchArgument(
+            "disable_capabilities",
+            default_value=moveit_config.move_group_capabilities["disable_capabilities"],
+        )
+    )
 
     # do not copy dynamics information from /joint_states to internal robot monitoring
     # default to false, because almost nothing in move_group relies on this information

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -108,7 +108,9 @@ class MoveItConfigs:
     # A dictionary that has the sensor 3d configuration parameters.
     sensors_3d: Dict = field(default_factory=dict)
     # A dictionary containing move_group's non-default capabilities.
-    move_group_capabilities: Dict = field(default_factory=dict)
+    move_group_capabilities: Dict = field(
+        default_factory=lambda: {"capabilities": "", "disable_capabilities": ""}
+    )
     # A dictionary containing the overridden position/velocity/acceleration limits.
     joint_limits: Dict = field(default_factory=dict)
     # A dictionary containing MoveItCpp related parameters.

moveit_core/constraint_samplers/test/pr2_arm_ik.h

@@ -37,7 +37,11 @@
 #pragma once
 
 #include <urdf_model/model.h>
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 #include <Eigen/Geometry>
 #include <Eigen/LU>  // provides LU decomposition
 #include <kdl/chainiksolver.hpp>

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h

@@ -50,7 +50,11 @@
 
 #include <kdl/chainfksolverpos_recursive.hpp>
 
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 
 #include <moveit/kinematics_base/kinematics_base.h>
 

moveit_core/robot_state/src/cartesian_interpolator.cpp

@@ -285,23 +285,22 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
     double wp_percentage_solved =
         computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
                              precision, validCallback, options, cost_function, link_offset);
+
+    std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
+    if (i > 0 && !waypoint_traj.empty())
+      std::advance(start, 1);
+    traj.insert(traj.end(), start, waypoint_traj.end());
+
     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
     {
       percentage_solved = static_cast<double>(i + 1) / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
     }
     else
     {
       percentage_solved += wp_percentage_solved / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
       break;
     }
+    start_state = traj.back().get();
   }
 
   return percentage_solved;
@@ -490,21 +489,19 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
         computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step,
                              NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset);
 #pragma GCC diagnostic pop
+
+    std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
+    if (i > 0 && !waypoint_path.empty())
+      std::advance(start, 1);
+    path.insert(path.end(), start, waypoint_path.end());
+
     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
     {
       percentage_solved = static_cast<double>((i + 1)) / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
-      if (i > 0 && !waypoint_path.empty())
-        std::advance(start, 1);
-      path.insert(path.end(), start, waypoint_path.end());
     }
     else
     {
       percentage_solved += wp_percentage_solved / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
-      if (i > 0 && !waypoint_path.empty())
-        std::advance(start, 1);
-      path.insert(path.end(), start, waypoint_path.end());
       break;
     }
   }

moveit_core/utils/include/moveit/utils/robot_model_test_utils.h

@@ -38,7 +38,11 @@
 #pragma once
 
 #include <srdfdom/srdf_writer.h>
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 #include <moveit/robot_model/robot_model.h>
 #include <geometry_msgs/msg/pose.hpp>
 

moveit_planners/moveit_planners/package.xml

@@ -19,10 +19,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <!--
   <exec_depend>chomp_motion_planner</exec_depend>
-  <exec_depend>moveit_planners_chomp</exec_depend>
-  -->
   <exec_depend>moveit_planners_ompl</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
   <exec_depend>pilz_industrial_motion_planner</exec_depend>

moveit_py/moveit/planning.pyi

@@ -16,7 +16,7 @@ class MoveItPy:
     def get_planning_component(self, *args, **kwargs) -> Any: ...
     def get_planning_scene_monitor(self, *args, **kwargs) -> Any: ...
     def get_robot_model(self, *args, **kwargs) -> Any: ...
-    def get_trajactory_execution_manager(self, *args, **kwargs) -> Any: ...
+    def get_trajectory_execution_manager(self, *args, **kwargs) -> Any: ...
     def shutdown(self, *args, **kwargs) -> Any: ...
 
 class MultiPipelinePlanRequestParameters:

moveit_ros/moveit_servo/src/servo.cpp

@@ -648,8 +648,8 @@ KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const
     bool have_current_state = false;
     while (rclcpp::ok() && !have_current_state)
     {
-      have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
-          rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */);
+      have_current_state =
+          planning_scene_monitor_->getStateMonitor()->waitForCurrentState(node_->now(), ROBOT_STATE_WAIT_TIME /* s */);
       if (!have_current_state)
       {
         RCLCPP_WARN(logger_, "Waiting for the current state");

moveit_ros/planning/CMakeLists.txt

@@ -49,8 +49,6 @@ set(THIS_PACKAGE_LIBRARIES
     moveit_collision_plugin_loader
     moveit_constraint_sampler_manager_loader
     moveit_cpp
-    moveit_default_planning_request_adapter_plugins
-    moveit_default_planning_response_adapter_plugins
     moveit_kinematics_plugin_loader
     moveit_plan_execution
     moveit_planning_pipeline
@@ -111,6 +109,16 @@ install(
   INCLUDES
   DESTINATION include/moveit_ros_planning)
 
+# install plugins as separate export set
+install(
+  TARGETS moveit_default_planning_response_adapter_plugins
+          moveit_default_planning_request_adapter_plugins
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include/moveit_ros_planning)
+
 ament_export_targets(moveit_ros_planningTargets HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 

moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h

@@ -38,7 +38,11 @@
 
 #include <moveit/macros/class_forward.h>
 #include <moveit/rdf_loader/synchronized_string_parameter.h>
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 #include <srdfdom/model.h>
 #include <rclcpp/rclcpp.hpp>
 

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -302,10 +302,6 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
   /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */
   void setStartStateToCurrentState();
 
-  /** \brief For pick/place operations, the name of the support surface is used to specify the fact that attached
-   * objects are allowed to touch the support surface */
-  void setSupportSurfaceName(const std::string& name);
-
   /**
    * \name Setting a joint state target (goal)
    *