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

.docker/source/Dockerfile

@@ -4,7 +4,7 @@
 # Downloads the moveit source code and install remaining debian dependencies
 
 ARG ROS_DISTRO=rolling
-FROM moveit/moveit2:${ROS_DISTRO}-ci-testing
+FROM moveit/moveit2:${ROS_DISTRO}-ci
 LABEL maintainer Robert Haschke [email protected]
 
 # Export ROS_UNDERLAY for downstream docker containers

.docker/tutorial-source/Dockerfile

@@ -0,0 +1,43 @@
+# syntax = docker/dockerfile:1.3
+
+# ghcr.io/moveit/moveit2:main-${ROS_DISTRO}-tutorial-source
+# Source build of the repos file from the tutorial site
+
+ARG ROS_DISTRO=humble
+
+FROM moveit/moveit2:${ROS_DISTRO}-ci
+LABEL maintainer Tyler Weaver [email protected]
+
+# Export ROS_UNDERLAY for downstream docker containers
+ENV ROS_UNDERLAY /root/ws_moveit/install
+WORKDIR $ROS_UNDERLAY/..
+
+# 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 --mount=type=cache,target=/root/.ccache/,sharing=locked \
+        # Enable ccache
+        PATH=/usr/lib/ccache:$PATH && \
+        # Checkout the tutorial repo
+        git clone -b ${ROS_DISTRO} https://github.com/moveit/moveit2_tutorials src/moveit2_tutorials && \
+        # Fetch required upstream sources for building
+        vcs import --skip-existing src < src/moveit2_tutorials/moveit2_tutorials.repos && \
+        # Source ROS install
+        . "/opt/ros/${ROS_DISTRO}/setup.sh" &&\
+        # Install dependencies from rosdep
+        apt-get -q update && \
+        rosdep update && \
+        DEBIAN_FRONTEND=noninteractive \
+        rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \
+        rm -rf /var/lib/apt/lists/* && \
+        # Build the workspace
+        colcon build \
+        --cmake-args "-DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --no-warn-unused-cli" \
+        --ament-cmake-args -DCMAKE_BUILD_TYPE=Release \
+        --event-handlers desktop_notification- status- && \
+        ccache -s && \
+        #
+        # Update /ros_entrypoint.sh to source our new workspace
+        sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.sh#g" /ros_entrypoint.sh

.github/workflows/ci.yaml

@@ -19,7 +19,7 @@ jobs:
           - IMAGE: humble-ci
             CCOV: true
             ROS_DISTRO: humble
-          - IMAGE: humble-ci-testing
+          - IMAGE: humble-ci
             ROS_DISTRO: humble
             IKFAST_TEST: true
             CLANG_TIDY: pedantic

.github/workflows/docker.yaml

@@ -97,49 +97,8 @@ jobs:
             ${{ env.GH_IMAGE }}
             ${{ env.DH_IMAGE }}
 
-  ci-testing:
-    strategy:
-      fail-fast: false
-      matrix:
-        ROS_DISTRO: [humble]
-    runs-on: ubuntu-latest
-    permissions:
-      packages: write
-      contents: read
-    env:
-      GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }}
-      DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }}
-      PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }}
-
-    steps:
-      - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
-      - name: Login to Github Container Registry
-        if: env.PUSH == 'true'
-        uses: docker/login-action@v3
-        with:
-          registry: ghcr.io
-          username: ${{ github.repository_owner }}
-          password: ${{ secrets.GITHUB_TOKEN }}
-      - name: Login to DockerHub
-        if: env.PUSH == 'true'
-        uses: docker/login-action@v3
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      - name: Build and Push
-        uses: docker/build-push-action@v5
-        with:
-          file: .docker/${{ github.job }}/Dockerfile
-          build-args: OUR_ROS_DISTRO=${{ matrix.ROS_DISTRO }}
-          push: ${{ env.PUSH }}
-          no-cache: true
-          tags: |
-            ${{ env.GH_IMAGE }}
-            ${{ env.DH_IMAGE }}
-
   source:
-    needs: ci-testing
+    needs: ci
     strategy:
       fail-fast: false
       matrix:

.github/workflows/docker_lint.yaml

@@ -16,7 +16,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        DOCKERFILE_PATH: [ci, ci-testing, release, source]
+        DOCKERFILE_PATH: [ci, release, source]
 
     name: Lint Dockerfiles
     runs-on: ubuntu-latest

.github/workflows/tutorial_docker.yaml

@@ -0,0 +1,95 @@
+name: "Tutorial Docker Images"
+
+on:
+  schedule:
+    # 5 PM UTC every Sunday
+    - cron:  '0 17 * * 6'
+  workflow_dispatch:
+  pull_request:
+  merge_group:
+  push:
+    branches:
+      - humble
+
+jobs:
+  tutorial-source:
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [humble]
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    env:
+      GH_IMAGE: ghcr.io/moveit/moveit2:humble-${{ matrix.ROS_DISTRO }}-${{ github.job }}
+      DH_IMAGE: moveit/moveit2:humble-${{ matrix.ROS_DISTRO }}-${{ github.job }}
+      PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }}
+
+    steps:
+      - uses: actions/checkout@v4
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      - name: Login to Github Container Registry
+        if: env.PUSH == 'true'
+        uses: docker/login-action@v3
+        with:
+          registry: ghcr.io
+          username: ${{ github.repository_owner }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+      - name: Login to DockerHub
+        if: env.PUSH == 'true'
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: "Remove .dockerignore"
+        run: rm .dockerignore  # enforce full source context
+      - name: Cache ccache
+        uses: actions/cache@v4
+        with:
+          path: .ccache
+          key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }}
+      - name: inject ccache into docker
+        uses: reproducible-containers/[email protected]
+        with:
+          cache-source: .ccache
+          cache-target: /root/.ccache/
+      - name: Build and Push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          file: .docker/${{ github.job }}/Dockerfile
+          build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }}
+          push: ${{ env.PUSH }}
+          cache-from: type=gha
+          cache-to: type=gha,mode=max
+          tags: |
+            ${{ env.GH_IMAGE }}
+            ${{ env.DH_IMAGE }}
+
+  delete_untagged:
+    runs-on: ubuntu-latest
+    needs:
+      - tutorial-source
+    steps:
+      - name: Delete Untagged Images
+        if: (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2')
+        uses: actions/github-script@v7
+        with:
+          github-token: ${{ secrets.DELETE_PACKAGES_TOKEN }}
+          script: |
+            const response = await github.request("GET /orgs/${{ env.OWNER }}/packages/container/${{ env.PACKAGE_NAME }}/versions", {
+                per_page: ${{ env.PER_PAGE }}
+              });
+            for(version of response.data) {
+                if (version.metadata.container.tags.length == 0) {
+                    console.log("delete " + version.id)
+                    const deleteResponse = await github.request("DELETE /orgs/${{ env.OWNER }}/packages/container/${{ env.PACKAGE_NAME }}/versions/" + version.id, { });
+                    console.log("status " + deleteResponse.status)
+                }
+            }
+        env:
+          OWNER: moveit
+          PACKAGE_NAME: moveit2
+          PER_PAGE: 100

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

@@ -95,7 +95,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
   RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");
 
   info.group_name = req.group_name;
-  std::string frame_id{ robot_model_->getModelFrame() };
   moveit::core::RobotState robot_state = scene->getCurrentState();
 
   // goal given in joint space
@@ -128,6 +127,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
   // goal given in Cartesian space
   else
   {
+    std::string frame_id;
     info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
     if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
         req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
@@ -140,39 +140,59 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
     {
       frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
     }
-    info.goal_pose = getConstraintPose(req.goal_constraints.front());
+
+    // goal pose with optional offset wrt. the planning frame
+    info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
+    frame_id = robot_model_->getModelFrame();
+
+    // check goal pose ik before Cartesian motion plan starts
+    std::map<std::string, double> ik_solution;
+    if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
+                       ik_solution))
+    {
+      // LCOV_EXCL_START
+      std::ostringstream os;
+      os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
+      throw CircInverseForGoalIncalculable(os.str());
+      // LCOV_EXCL_STOP // not able to trigger here since lots of checks before
+      // are in place
+    }
   }
 
   computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
 
-  // check goal pose ik before Cartesian motion plan starts
-  std::map<std::string, double> ik_solution;
-  if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
-                     ik_solution))
+  // center point with wrt. the planning frame
+  std::string center_point_frame_id;
+
+  info.circ_path_point.first = req.path_constraints.name;
+  if (req.path_constraints.position_constraints.front().header.frame_id.empty())
   {
-    // LCOV_EXCL_START
-    std::ostringstream os;
-    os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
-    throw CircInverseForGoalIncalculable(os.str());
-    // LCOV_EXCL_STOP // not able to trigger here since lots of checks before
-    // are in place
+    RCLCPP_WARN(LOGGER, "Frame id is not set in position constraints of "
+                        "path. Use model frame as default");
+    center_point_frame_id = robot_model_->getModelFrame();
   }
-  info.circ_path_point.first = req.path_constraints.name;
+  else
+  {
+    center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id;
+  }
+
+  Eigen::Isometry3d center_point_pose;
+  tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(),
+               center_point_pose);
+
+  center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose;
+
   if (!req.goal_constraints.front().position_constraints.empty())
   {
     const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front();
-    info.circ_path_point.second =
-        getConstraintPose(
-            req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
-            goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset)
-            .translation();
+    geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation()));
+    info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation,
+                                                    goal.position_constraints.front().target_point_offset)
+                                      .translation();
   }
   else
   {
-    Eigen::Vector3d circ_path_point;
-    tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
-                 circ_path_point);
-    info.circ_path_point.second = circ_path_point;
+    info.circ_path_point.second = center_point_pose.translation();
   }
 }