Merge branch 'main' into ch3/trajectory-cache-refactor

.docker/ci/Dockerfile

@@ -18,7 +18,7 @@ COPY . src/moveit2
 RUN \
     # Update apt package list as previous containers clear the cache
     apt-get -q update && \
-    apt-get -q -y upgrade --with-new-pkgs && \
+    apt-get -q -y upgrade && \
     #
     # Install some base dependencies
     apt-get -q install --no-install-recommends -y \

.github/workflows/ci.yaml

@@ -26,10 +26,10 @@ jobs:
             CLANG_TIDY: pedantic
           - IMAGE: humble-ci
             ROS_DISTRO: humble
-          - IMAGE: humble-ci-testing
-            ROS_DISTRO: humble
           - IMAGE: jazzy-ci
             ROS_DISTRO: jazzy
+          - IMAGE: jazzy-ci-testing
+            ROS_DISTRO: jazzy
     env:
       # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option
       # https://stackoverflow.com/a/41673702
@@ -167,7 +167,7 @@ jobs:
           workdir: ${{ env.BASEDIR }}/target_ws
           ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
       - name: Upload codecov report
-        uses: codecov/codecov-action@v4
+        uses: codecov/codecov-action@v5
         if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
         with:
           files: ${{ env.BASEDIR }}/target_ws/coverage.info

MIGRATION.md

@@ -3,6 +3,8 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
+- [11/2024] Added flags to control padding to CollisionRequest. This change deprecates PlanningScene::checkCollisionUnpadded(..) functions. Please use PlanningScene::checkCollision(..) with a req.pad_environment_collisions = false;
+
 - [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse.
 The constructors have been replaced by builder methods so that errors can be communicated. Paths and trajectories now need to be created with `Path::Create()` and `Trajectory::Create()`. These methods now return an `std::optional` that needs to be checked for a valid value. `Trajectory` no longer has the `isValid()` method. If it's invalid, `Trajectory::Create()` will return `std::nullopt`. Finally, `Path` now takes the list of input waypoints as `std::vector`, instead of `std::list`.
 - [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK

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

@@ -150,6 +150,12 @@ struct CollisionRequest
    * are included. */
   std::string group_name = "";
 
+  /** \brief If true, use padded collision environment */
+  bool pad_environment_collisions = true;
+
+  /** \brief If true, do self collision check with padded robot links */
+  bool pad_self_collisions = false;
+
   /** \brief If true, compute proximity distance */
   bool distance = false;
 

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

@@ -347,19 +347,12 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);
 
   /** \brief Check whether the current state is in collision. The current state is expected to be updated. */
-  void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const
-  {
-    checkCollision(req, res, getCurrentState());
-  }
+  void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes
       a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */
   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
-                      moveit::core::RobotState& robot_state) const
-  {
-    robot_state.updateCollisionBodyTransforms();
-    checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state));
-  }
+                      moveit::core::RobotState& robot_state) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e
    * robot_state are
@@ -372,11 +365,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
       a non-const \e robot_state and updates its link transforms if needed. */
   void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                       moveit::core::RobotState& robot_state,
-                      const collision_detection::AllowedCollisionMatrix& acm) const
-  {
-    robot_state.updateCollisionBodyTransforms();
-    checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
-  }
+                      const collision_detection::AllowedCollisionMatrix& acm) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given
       allowed collision matrix (\e acm). */
@@ -387,99 +376,69 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
   /** \brief Check whether the current state is in collision,
       but use a collision_detection::CollisionRobot instance that has no padding.
       Since the function is non-const, the current state transforms are also updated if needed. */
-  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
-                              collision_detection::CollisionResult& res);
+  [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
+  checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);
 
   /** \brief Check whether the current state is in collision,
       but use a collision_detection::CollisionRobot instance that has no padding.  */
-  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
-                              collision_detection::CollisionResult& res) const
-  {
-    checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix());
-  }
+  [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
+  checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
+                         collision_detection::CollisionResult& res) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision,
       but use a collision_detection::CollisionRobot instance that has no padding.  */
-  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
-                              collision_detection::CollisionResult& res,
-                              const moveit::core::RobotState& robot_state) const
-  {
-    checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
-  }
+  [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
+  checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
+                         const moveit::core::RobotState& robot_state) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision,
       but use a collision_detection::CollisionRobot instance that has no padding.
       Update the link transforms of \e robot_state if needed. */
-  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
-                              collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state) const
-  {
-    robot_state.updateCollisionBodyTransforms();
-    checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state),
-                           getAllowedCollisionMatrix());
-  }
+  [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
+  checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
+                         moveit::core::RobotState& robot_state) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given
       allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding.
       This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */
-  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
-                              collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state,
-                              const collision_detection::AllowedCollisionMatrix& acm) const
-  {
-    robot_state.updateCollisionBodyTransforms();
-    checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
-  }
+  [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
+  checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
+                         moveit::core::RobotState& robot_state,
+                         const collision_detection::AllowedCollisionMatrix& acm) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given
       allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding.  */
-  void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
-                              collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state,
-                              const collision_detection::AllowedCollisionMatrix& acm) const;
+  [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
+  checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
+                         const moveit::core::RobotState& robot_state,
+                         const collision_detection::AllowedCollisionMatrix& acm) const;
 
   /** \brief Check whether the current state is in self collision */
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);
 
   /** \brief Check whether the current state is in self collision */
   void checkSelfCollision(const collision_detection::CollisionRequest& req,
-                          collision_detection::CollisionResult& res) const
-  {
-    checkSelfCollision(req, res, getCurrentState());
-  }
+                          collision_detection::CollisionResult& res) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in self collision */
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
-                          moveit::core::RobotState& robot_state) const
-  {
-    robot_state.updateCollisionBodyTransforms();
-    checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
-  }
+                          moveit::core::RobotState& robot_state) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in self collision */
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
-                          const moveit::core::RobotState& robot_state) const
-  {
-    // do self-collision checking with the unpadded version of the robot
-    getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
-  }
+                          const moveit::core::RobotState& robot_state) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given
       allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                           moveit::core::RobotState& robot_state,
-                          const collision_detection::AllowedCollisionMatrix& acm) const
-  {
-    robot_state.updateCollisionBodyTransforms();
-    checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
-  }
+                          const collision_detection::AllowedCollisionMatrix& acm) const;
 
   /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given
       allowed collision matrix (\e acm) */
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                           const moveit::core::RobotState& robot_state,
-                          const collision_detection::AllowedCollisionMatrix& acm) const
-  {
-    // do self-collision checking with the unpadded version of the robot
-    getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
-  }
+                          const collision_detection::AllowedCollisionMatrix& acm) const;
 
   /** \brief Get the names of the links that are involved in collisions for the current state */
   void getCollidingLinks(std::vector<std::string>& links);