Merge branch 'main' into capabilities

.github/workflows/ci.yaml

@@ -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_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_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/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})