diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile deleted file mode 100644 index 45c18e07d6..0000000000 --- a/.docker/ci-testing/Dockerfile +++ /dev/null @@ -1,68 +0,0 @@ -# ghcr.io/ros-planning/moveit2:${OUR_ROS_DISTRO}-ci-testing -# CI image using the ROS testing repository - -FROM osrf/ros2:testing -LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de - -ENV TERM xterm - -# 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--ros-base here to mimic the behavior of the ros:-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/* diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 2b9082885e..d665d02ab2 100644 --- a/.docker/source/Dockerfile +++ b/.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 rhaschke@techfak.uni-bielefeld.de # Export ROS_UNDERLAY for downstream docker containers diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile new file mode 100644 index 0000000000..11c0ae738e --- /dev/null +++ b/.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 tyler@picknik.ai + +# 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 diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 78a7067fa6..a1c124afc0 100644 --- a/.github/workflows/ci.yaml +++ b/.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 diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index cb02273b64..3c934287b4 100644 --- a/.github/workflows/docker.yaml +++ b/.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: diff --git a/.github/workflows/docker_lint.yaml b/.github/workflows/docker_lint.yaml index 2806d563ad..14c1cfc363 100644 --- a/.github/workflows/docker_lint.yaml +++ b/.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 diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml new file mode 100644 index 0000000000..c922f1a995 --- /dev/null +++ b/.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/buildkit-cache-dance@v3.1.2 + 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 diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index cd4e6b1de5..d3a145ca8f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/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 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 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(); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index bef1f5132f..0caf4b6327 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -72,7 +72,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin 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 @@ -101,6 +100,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // 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()) @@ -113,20 +114,25 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); - } - computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); + // 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 ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) - { - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw LinInverseForGoalIncalculable(os.str()); + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); + } } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index c1a0e62b8a..fe6b096c5d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -227,11 +227,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin // solve the ik else { - Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front()); - if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, - goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + 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()) + { + RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + + // 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 start + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + info.goal_joint_position)) { - throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw PtpNoIkSolutionForGoalPose(os.str()); } } }