Merge branch 'humble' into mergify/bp/humble/pr-1889

.docker/ci-testing/Dockerfile

@@ -1,16 +1,68 @@
-# ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing
+# ghcr.io/ros-planning/moveit2:${OUR_ROS_DISTRO}-ci-testing
 # CI image using the ROS testing repository
 
-ARG ROS_DISTRO=rolling
-FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci
+FROM osrf/ros2:testing
 LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
-# 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-shadow-fixed and clean apt-cache within one RUN command
-RUN apt-get -qq update && \
-    apt-get -qq upgrade && \
+# Overwrite the ROS_DISTRO set in osrf/ros2:testing to the distro tied to this Dockerfile (OUR_ROS_DISTRO).
+# In case ROS_DISTRO is now different from what was set in osrf/ros2:testing, run `rosdep update` again
+# to get any missing dependencies.
+# https://docs.docker.com/engine/reference/builder/#using-arg-variables explains why ARG and ENV can't have
+# the same name (ROS_DISTRO is an ENV in the osrf/ros2:testing image).
+ARG OUR_ROS_DISTRO=rolling
+ENV ROS_DISTRO=${OUR_ROS_DISTRO}
+RUN rosdep update --rosdistro $ROS_DISTRO
+
+# 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/*

.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 && \
+    apt-get -q -y upgrade --with-new-pkgs && \
     #
     # Install some base dependencies
     apt-get -q install --no-install-recommends -y \

.docker/source/Dockerfile

@@ -4,7 +4,7 @@
 # Downloads the moveit source code and install remaining debian dependencies
 
 ARG ROS_DISTRO=rolling
-FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing
+FROM moveit/moveit2:${ROS_DISTRO}-ci-testing
 LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
 # Export ROS_UNDERLAY for downstream docker containers

.github/workflows/ci.yaml

@@ -23,12 +23,18 @@ jobs:
             ROS_DISTRO: humble
             IKFAST_TEST: true
             CLANG_TIDY: pedantic
+            # Silent gmock/gtest warnings by picking more recent googletest version
+            AFTER_BUILD_UPSTREAM_WORKSPACE: |
+              git clone --depth 1 --quiet --branch 1.11.9000 https://github.com/ament/googletest "${BASEDIR}/upstream_ws/src/googletest"
+              builder_run_build "/opt/ros/${ROS_DISTRO}" "${BASEDIR}/upstream_ws" --packages-select gtest_vendor gmock_vendor
     env:
       CXXFLAGS: >-
         -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
-      DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
-      UPSTREAM_WORKSPACE: moveit2.repos $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f)
+      DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
+      UPSTREAM_WORKSPACE: >
+        moveit2.repos
+        $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f)
       # Pull any updates to the upstream workspace (after restoring it from cache)
       AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
       # Uninstall binaries that are duplicated in the .repos file
@@ -40,13 +46,13 @@ jobs:
       BEFORE_BUILD_UPSTREAM_WORKSPACE: ccache -z
       AFTER_BUILD_TARGET_WORKSPACE: ccache -s
       # Changing linker to lld as ld has a behavior where it takes a long time to finish
-      # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings)
+      # Compile CCOV with Debug. Enable -Werror.
       TARGET_CMAKE_ARGS: >
         -DCMAKE_EXE_LINKER_FLAGS=-fuse-ld=lld
         -DCMAKE_SHARED_LINKER_FLAGS=-fuse-ld=lld
         -DCMAKE_MODULE_LINKER_FLAGS=-fuse-ld=lld
         -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}
-        -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
+        -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
       UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''"
       DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra"
       CCACHE_DIR: ${{ github.workspace }}/.ccache

.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,10 +128,10 @@ 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 }}
+          build-args: OUR_ROS_DISTRO=${{ matrix.ROS_DISTRO }}
           push: ${{ env.PUSH }}
           no-cache: true
           tags: |
@@ -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

Doxyfile

@@ -2081,7 +2081,7 @@ TAGFILES               =
 # tag file that is based on the input files it reads. See section "Linking to
 # external documentation" for more information about the usage of tag files.
 
-GENERATE_TAGFILE       =
+GENERATE_TAGFILE       = $(DOXYGEN_OUTPUT_DIRECTORY)/MoveIt.tag
 
 # If the ALLEXTERNALS tag is set to YES, all external class will be listed in
 # the class index. If set to NO, only the inherited external classes will be

moveit_configs_utils/default_configs/chomp_planning.yaml

@@ -2,7 +2,7 @@ planning_plugin: chomp_interface/CHOMPPlanner
 enable_failure_recovery: true
 jiggle_fraction: 0.05
 request_adapters: >-
-    default_planner_request_adapters/AddTimeParameterization
+    default_planner_request_adapters/AddTimeOptimalParameterization
     default_planner_request_adapters/ResolveConstraintFrames
     default_planner_request_adapters/FixWorkspaceBounds
     default_planner_request_adapters/FixStartStateBounds

moveit_configs_utils/default_configs/ompl_planning.yaml

@@ -2,7 +2,7 @@ planning_plugin: ompl_interface/OMPLPlanner
 start_state_max_bounds_error: 0.1
 jiggle_fraction: 0.05
 request_adapters: >-
-    default_planner_request_adapters/AddTimeParameterization
+    default_planner_request_adapters/AddTimeOptimalParameterization
     default_planner_request_adapters/ResolveConstraintFrames
     default_planner_request_adapters/FixWorkspaceBounds
     default_planner_request_adapters/FixStartStateBounds

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -58,7 +58,10 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch_param_builder import ParameterBuilder, load_yaml, load_xacro
-
+from launch_param_builder.utils import ParameterBuilderFileNotFoundError
+from moveit_configs_utils.substitutions import Xacro
+from launch.some_substitutions_type import SomeSubstitutionsType
+from launch_ros.parameter_descriptions import ParameterValue
 
 moveit_configs_utils_path = Path(get_package_share_directory("moveit_configs_utils"))
 
@@ -125,9 +128,10 @@ def to_dict(self):
         parameters.update(self.joint_limits)
         parameters.update(self.moveit_cpp)
         # Update robot_description_planning with pilz cartesian limits
-        parameters["robot_description_planning"].update(
-            self.pilz_cartesian_limits["robot_description_planning"]
-        )
+        if self.pilz_cartesian_limits:
+            parameters["robot_description_planning"].update(
+                self.pilz_cartesian_limits["robot_description_planning"]
+            )
         return parameters
 
 
@@ -198,7 +202,11 @@ def __init__(
 
         self.__robot_description = robot_description
 
-    def robot_description(self, file_path: Optional[str] = None, mappings: dict = None):
+    def robot_description(
+        self,
+        file_path: Optional[str] = None,
+        mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] = None,
+    ):
         """Load robot description.
 
         :param file_path: Absolute or relative path to the URDF file (w.r.t. robot_name_moveit_config).
@@ -209,29 +217,65 @@ def robot_description(self, file_path: Optional[str] = None, mappings: dict = No
             robot_description_file_path = self.__urdf_package / self.__urdf_file_path
         else:
             robot_description_file_path = self._package_path / file_path
-        self.__moveit_configs.robot_description = {
-            self.__robot_description: load_xacro(
-                robot_description_file_path, mappings=mappings
-            )
-        }
+        if (mappings is None) or all(
+            (isinstance(key, str) and isinstance(value, str))
+            for key, value in mappings.items()
+        ):
+            try:
+                self.__moveit_configs.robot_description = {
+                    self.__robot_description: load_xacro(
+                        robot_description_file_path, mappings=mappings
+                    )
+                }
+            except ParameterBuilderFileNotFoundError as e:
+                logging.warning(f"\x1b[33;21m{e}\x1b[0m")
+                logging.warning(
+                    f"\x1b[33;21mThe robot description will be loaded from /robot_description topic \x1b[0m"
+                )
+
+        else:
+            self.__moveit_configs.robot_description = {
+                self.__robot_description: ParameterValue(
+                    Xacro(str(robot_description_file_path), mappings=mappings),
+                    value_type=str,
+                )
+            }
         return self
 
     def robot_description_semantic(
-        self, file_path: Optional[str] = None, mappings: dict = None
+        self,
+        file_path: Optional[str] = None,
+        mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] = None,
     ):
         """Load semantic robot description.
 
         :param file_path: Absolute or relative path to the SRDF file (w.r.t. robot_name_moveit_config).
         :param mappings: mappings to be passed when loading the xacro file.
         :return: Instance of MoveItConfigsBuilder with robot_description_semantic loaded.
         """
-        self.__moveit_configs.robot_description_semantic = {
-            self.__robot_description
-            + "_semantic": load_xacro(
-                self._package_path / (file_path or self.__srdf_file_path),
-                mappings=mappings,
-            )
-        }
+
+        if (mappings is None) or all(
+            (isinstance(key, str) and isinstance(value, str))
+            for key, value in mappings.items()
+        ):
+            self.__moveit_configs.robot_description_semantic = {
+                self.__robot_description
+                + "_semantic": load_xacro(
+                    self._package_path / (file_path or self.__srdf_file_path),
+                    mappings=mappings,
+                )
+            }
+        else:
+            self.__moveit_configs.robot_description_semantic = {
+                self.__robot_description
+                + "_semantic": ParameterValue(
+                    Xacro(
+                        str(self._package_path / (file_path or self.__srdf_file_path)),
+                        mappings=mappings,
+                    ),
+                    value_type=str,
+                )
+            }
         return self
 
     def robot_description_kinematics(self, file_path: Optional[str] = None):

moveit_configs_utils/moveit_configs_utils/substitutions/__init__.py

@@ -0,0 +1 @@
+from .xacro import Xacro