Merge branch 'main' into pr-totg-mimic

.clang-tidy

@@ -52,8 +52,6 @@ CheckOptions:
     value:           UPPER_CASE
   - key:             readability-identifier-naming.StaticConstantCase
     value:           UPPER_CASE
-  - key:             readability-identifier-naming.ClassConstantCase
-    value:           UPPER_CASE
-  - key:             readability-identifier-naming.GlobalVariableCase
+  - key:             readability-identifier-naming.GlobalConstantCase
     value:           UPPER_CASE
 ...

.docker/ci-testing/Dockerfile

@@ -2,15 +2,59 @@
 # CI image using the ROS testing repository
 
 ARG ROS_DISTRO=rolling
-FROM moveit/moveit2:${ROS_DISTRO}-ci
+FROM osrf/ros2:testing
 LABEL maintainer Robert Haschke [email protected]
 
-# Switch to ros-testing
-RUN echo "deb http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2-latest.list
+ENV TERM xterm
 
-# Upgrade packages to ros-testing and clean apt-cache within one RUN command
-RUN apt-get -qq update && \
-    apt-get -qq upgrade && \
+# Install ROS 2 base packages and build tools
+# We are installing ros-<distro>-ros-base here to mimic the behavior of the ros:<distro>-ros-base images.
+# This step is split into a separate layer so that we can rely on cached dependencies instead of having
+# to install them with every new build. The testing image and packages will only update every couple weeks.
+RUN \
+    # Update apt package list as previous containers clear the cache
+    apt-get -q update && \
+    apt-get -q -y upgrade && \
+    #
+    # Install base dependencies
+    apt-get -q install --no-install-recommends -y \
+        # Some basic requirements
+        wget git sudo curl \
+        # Preferred build tools
+        clang clang-format-14 clang-tidy clang-tools \
+        ccache \
+        ros-"$ROS_DISTRO"-ros-base && \
+    #
+    # Clear apt-cache to reduce image size
+    rm -rf /var/lib/apt/lists/*
+
+# Setup (temporary) ROS workspace
+WORKDIR /root/ws_moveit
+
+# Copy MoveIt sources from docker context
+COPY . src/moveit2
+
+# Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
+# https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers
+RUN \
+    # Update apt package list as previous containers clear the cache
+    apt-get -q update && \
+    apt-get -q -y upgrade && \
+    #
+    # Globally disable git security
+    # https://github.blog/2022-04-12-git-security-vulnerability-announced
+    git config --global --add safe.directory "*" && \
+    #
+    # Fetch all dependencies from moveit2.repos
+    vcs import src < src/moveit2/moveit2.repos && \
+    if [ -r src/moveit2/moveit2_"$ROS_DISTRO".repos ] ; then vcs import src < src/moveit2/moveit2_"$ROS_DISTRO".repos ; fi && \
+    #
+    # Download all dependencies of MoveIt
+    rosdep update && \
+    DEBIAN_FRONTEND=noninteractive \
+    rosdep install -y --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" --as-root=apt:false && \
+    # Remove the source code from this container
+    rm -rf src && \
     #
     # Clear apt-cache to reduce image size
     rm -rf /var/lib/apt/lists/*

.github/workflows/docker.yaml

@@ -46,7 +46,7 @@ jobs:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: Build and Push
-        uses: docker/build-push-action@v3
+        uses: docker/build-push-action@v4
         with:
           file: .docker/${{ github.job }}/Dockerfile
           build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }}
@@ -87,7 +87,7 @@ jobs:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: Build and Push
-        uses: docker/build-push-action@v3
+        uses: docker/build-push-action@v4
         with:
           file: .docker/${{ github.job }}/Dockerfile
           build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }}
@@ -98,7 +98,6 @@ jobs:
             ${{ env.DH_IMAGE }}
 
   ci-testing:
-    needs: ci
     strategy:
       fail-fast: false
       matrix:
@@ -129,7 +128,7 @@ jobs:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: Build and Push
-        uses: docker/build-push-action@v3
+        uses: docker/build-push-action@v4
         with:
           file: .docker/${{ github.job }}/Dockerfile
           build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }}
@@ -174,7 +173,7 @@ jobs:
       - name: "Remove .dockerignore"
         run: rm .dockerignore  # enforce full source context
       - name: Build and Push
-        uses: docker/build-push-action@v3
+        uses: docker/build-push-action@v4
         with:
           context: .
           file: .docker/${{ github.job }}/Dockerfile

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h

@@ -138,28 +138,28 @@ class CollisionObjectWrapper : public btCollisionObject
   bool m_enabled{ true };
 
   /** \brief The robot links the collision objects is allowed to touch */
-  std::set<std::string> m_touch_links;
+  std::set<std::string> touch_links;
 
   /** @brief Get the collision object name */
   const std::string& getName() const
   {
-    return m_name;
+    return name_;
   }
 
   /** @brief Get a user defined type */
   const collision_detection::BodyType& getTypeID() const
   {
-    return m_type_id;
+    return type_id_;
   }
 
   /** \brief Check if two CollisionObjectWrapper objects point to the same source object
    *  \return True if same objects, false otherwise */
   bool sameObject(const CollisionObjectWrapper& other) const
   {
-    return m_name == other.m_name && m_type_id == other.m_type_id && m_shapes.size() == other.m_shapes.size() &&
-           m_shape_poses.size() == other.m_shape_poses.size() &&
-           std::equal(m_shapes.begin(), m_shapes.end(), other.m_shapes.begin()) &&
-           std::equal(m_shape_poses.begin(), m_shape_poses.end(), other.m_shape_poses.begin(),
+    return name_ == other.name_ && type_id_ == other.type_id_ && shapes_.size() == other.shapes_.size() &&
+           shape_poses_.size() == other.shape_poses_.size() &&
+           std::equal(shapes_.begin(), shapes_.end(), other.shapes_.begin()) &&
+           std::equal(shape_poses_.begin(), shape_poses_.end(), other.shape_poses_.begin(),
                       [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); });
   }
 
@@ -181,14 +181,14 @@ class CollisionObjectWrapper : public btCollisionObject
   std::shared_ptr<CollisionObjectWrapper> clone()
   {
     std::shared_ptr<CollisionObjectWrapper> clone_cow(
-        new CollisionObjectWrapper(m_name, m_type_id, m_shapes, m_shape_poses, m_collision_object_types, m_data));
+        new CollisionObjectWrapper(name_, type_id_, shapes_, shape_poses_, collision_object_types_, data_));
     clone_cow->setCollisionShape(getCollisionShape());
     clone_cow->setWorldTransform(getWorldTransform());
     clone_cow->m_collisionFilterGroup = m_collisionFilterGroup;
     clone_cow->m_collisionFilterMask = m_collisionFilterMask;
     clone_cow->m_enabled = m_enabled;
     clone_cow->setBroadphaseHandle(nullptr);
-    clone_cow->m_touch_links = m_touch_links;
+    clone_cow->touch_links = touch_links;
     clone_cow->setContactProcessingThreshold(getContactProcessingThreshold());
     return clone_cow;
   }
@@ -197,14 +197,14 @@ class CollisionObjectWrapper : public btCollisionObject
   template <class T>
   void manage(T* t)
   {
-    m_data.push_back(std::shared_ptr<T>(t));
+    data_.push_back(std::shared_ptr<T>(t));
   }
 
   /** \brief Manage memory of a shared pointer shape */
   template <class T>
   void manage(std::shared_ptr<T> t)
   {
-    m_data.push_back(t);
+    data_.push_back(t);
   }
 
 protected:
@@ -216,21 +216,21 @@ class CollisionObjectWrapper : public btCollisionObject
                          const std::vector<std::shared_ptr<void>>& data);
 
   /** \brief The name of the object, must be unique. */
-  std::string m_name;
+  std::string name_;
 
-  collision_detection::BodyType m_type_id;
+  collision_detection::BodyType type_id_;
 
   /** @brief The shapes that define the collision object */
-  std::vector<shapes::ShapeConstPtr> m_shapes;
+  std::vector<shapes::ShapeConstPtr> shapes_;
 
   /** @brief The poses of the shapes, must be same length as m_shapes */
-  AlignedVector<Eigen::Isometry3d> m_shape_poses;
+  AlignedVector<Eigen::Isometry3d> shape_poses_;
 
   /** @brief The shape collision object type to be used. */
-  std::vector<CollisionObjectType> m_collision_object_types;
+  std::vector<CollisionObjectType> collision_object_types_;
 
   /** @brief Manages the collision shape pointer so they get destroyed */
-  std::vector<std::shared_ptr<void>> m_data;
+  std::vector<std::shared_ptr<void>> data_;
 };
 
 /** @brief Casted collision shape used for checking if an object is collision free between two discrete poses
@@ -244,24 +244,23 @@ struct CastHullShape : public btConvexShape
   btConvexShape* m_shape;
 
   /** \brief Transformation from the first pose to the second pose */
-  btTransform m_shape_transform;
+  btTransform shape_transform;
 
-  CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), m_shape_transform(t01)
+  CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), shape_transform(t01)
   {
     m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE;
   }
 
   void updateCastTransform(const btTransform& cast_transform)
   {
-    m_shape_transform = cast_transform;
+    shape_transform = cast_transform;
   }
 
   /** \brief From both shape poses computes the support vertex and returns the larger one. */
   btVector3 localGetSupportingVertex(const btVector3& vec) const override
   {
     btVector3 support_vector_0 = m_shape->localGetSupportingVertex(vec);
-    btVector3 support_vector_1 =
-        m_shape_transform * m_shape->localGetSupportingVertex(vec * m_shape_transform.getBasis());
+    btVector3 support_vector_1 = shape_transform * m_shape->localGetSupportingVertex(vec * shape_transform.getBasis());
     return (vec.dot(support_vector_0) > vec.dot(support_vector_1)) ? support_vector_0 : support_vector_1;
   }
 
@@ -279,7 +278,7 @@ struct CastHullShape : public btConvexShape
   {
     m_shape->getAabb(transform_world, aabbMin, aabbMax);
     btVector3 min1, max1;
-    m_shape->getAabb(transform_world * m_shape_transform, min1, max1);
+    m_shape->getAabb(transform_world * shape_transform, min1, max1);
     aabbMin.setMin(min1);
     aabbMax.setMax(max1);
   }
@@ -471,7 +470,7 @@ inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObject
   const CastHullShape* shape = static_cast<const CastHullShape*>(first_col_obj_wrap->getCollisionShape());
 
   tf_world0 = first_col_obj_wrap->getWorldTransform();
-  tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->m_shape_transform;
+  tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->shape_transform;
 
   // transform normals into pose local coordinate systems
   btVector3 normal_local0 = normal_world_from_cast * tf_world0.getBasis();

moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp

@@ -457,21 +457,21 @@ bool BroadphaseFilterCallback::needBroadphaseCollision(btBroadphaseProxy* proxy0
   if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED &&
       cow1->getTypeID() == collision_detection::BodyType::ROBOT_LINK)
   {
-    if (cow0->m_touch_links.find(cow1->getName()) != cow0->m_touch_links.end())
+    if (cow0->touch_links.find(cow1->getName()) != cow0->touch_links.end())
       return false;
   }
 
   if (cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED &&
       cow0->getTypeID() == collision_detection::BodyType::ROBOT_LINK)
   {
-    if (cow1->m_touch_links.find(cow0->getName()) != cow1->m_touch_links.end())
+    if (cow1->touch_links.find(cow0->getName()) != cow1->touch_links.end())
       return false;
   }
 
   if (cow0->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED &&
       cow1->getTypeID() == collision_detection::BodyType::ROBOT_ATTACHED)
   {
-    if (cow0->m_touch_links == cow1->m_touch_links)
+    if (cow0->touch_links == cow1->touch_links)
       return false;
   }
 
@@ -546,11 +546,11 @@ CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const co
                                                const AlignedVector<Eigen::Isometry3d>& shape_poses,
                                                const std::vector<CollisionObjectType>& collision_object_types,
                                                bool active)
-  : m_name(name)
-  , m_type_id(type_id)
-  , m_shapes(shapes)
-  , m_shape_poses(shape_poses)
-  , m_collision_object_types(collision_object_types)
+  : name_(name)
+  , type_id_(type_id)
+  , shapes_(shapes)
+  , shape_poses_(shape_poses)
+  , collision_object_types_(collision_object_types)
 {
   if (shapes.empty() || shape_poses.empty() ||
       (shapes.size() != shape_poses.size() || collision_object_types.empty() ||
@@ -575,32 +575,31 @@ CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const co
 
   if (shapes.size() == 1)
   {
-    btCollisionShape* shape = createShapePrimitive(m_shapes[0], collision_object_types[0], this);
+    btCollisionShape* shape = createShapePrimitive(shapes_[0], collision_object_types[0], this);
     shape->setMargin(BULLET_MARGIN);
     manage(shape);
     setCollisionShape(shape);
-    setWorldTransform(convertEigenToBt(m_shape_poses[0]));
+    setWorldTransform(convertEigenToBt(shape_poses_[0]));
   }
   else
   {
-    btCompoundShape* compound =
-        new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(m_shapes.size()));
+    btCompoundShape* compound = new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(shapes_.size()));
     manage(compound);
     // margin on compound seems to have no effect when positive but has an effect when negative
     compound->setMargin(BULLET_MARGIN);
     setCollisionShape(compound);
 
-    setWorldTransform(convertEigenToBt(m_shape_poses[0]));
-    Eigen::Isometry3d inv_world = m_shape_poses[0].inverse();
+    setWorldTransform(convertEigenToBt(shape_poses_[0]));
+    Eigen::Isometry3d inv_world = shape_poses_[0].inverse();
 
-    for (std::size_t j = 0; j < m_shapes.size(); ++j)
+    for (std::size_t j = 0; j < shapes_.size(); ++j)
     {
-      btCollisionShape* subshape = createShapePrimitive(m_shapes[j], collision_object_types[j], this);
+      btCollisionShape* subshape = createShapePrimitive(shapes_[j], collision_object_types[j], this);
       if (subshape != nullptr)
       {
         manage(subshape);
         subshape->setMargin(BULLET_MARGIN);
-        btTransform geom_trans = convertEigenToBt(inv_world * m_shape_poses[j]);
+        btTransform geom_trans = convertEigenToBt(inv_world * shape_poses_[j]);
         compound->addChildShape(geom_trans, subshape);
       }
     }
@@ -614,20 +613,20 @@ CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const co
                                                const std::set<std::string>& touch_links)
   : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, true)
 {
-  m_touch_links = touch_links;
+  this->touch_links = touch_links;
 }
 
 CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id,
                                                const std::vector<shapes::ShapeConstPtr>& shapes,
                                                const AlignedVector<Eigen::Isometry3d>& shape_poses,
                                                const std::vector<CollisionObjectType>& collision_object_types,
                                                const std::vector<std::shared_ptr<void>>& data)
-  : m_name(name)
-  , m_type_id(type_id)
-  , m_shapes(shapes)
-  , m_shape_poses(shape_poses)
-  , m_collision_object_types(collision_object_types)
-  , m_data(data)
+  : name_(name)
+  , type_id_(type_id)
+  , shapes_(shapes)
+  , shape_poses_(shape_poses)
+  , collision_object_types_(collision_object_types)
+  , data_(data)
 {
 }
 }  // namespace collision_detection_bullet

moveit_core/planning_request_adapter/src/planning_request_adapter.cpp

@@ -42,7 +42,7 @@
 
 namespace planning_request_adapter
 {
-rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("planning_request_adapter");
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.planning_request_adapter");
 
 namespace
 {