Merge branch 'main' into pr-refactor_pipeline_chain

.docker/tutorial-source/Dockerfile

@@ -0,0 +1,42 @@
+# syntax = docker/dockerfile:1.3
+
+# ghcr.io/ros-planning/moveit2:main-${ROS_DISTRO}-tutorial-source
+# Source build of the repos file from the tutorail site
+
+ARG ROS_DISTRO=rolling
+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 https://github.com/ros-planning/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/tutorial_docker.yaml

@@ -0,0 +1,94 @@
+name: "Tutorial Docker Images"
+
+on:
+  schedule:
+    # 5 PM UTC every Sunday
+    - cron:  '0 17 * * 6'
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+      - main
+
+jobs:
+  tutorial-source:
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [rolling, humble, iron]
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    env:
+      GH_IMAGE: ghcr.io/ros-planning/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }}
+      DH_IMAGE: moveit/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }}
+      PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/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@v3
+        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@v5
+        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 == 'ros-planning/moveit2')
+        uses: actions/github-script@v6
+        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: ros-planning
+          PACKAGE_NAME: moveit2
+          PER_PAGE: 100

.gitignore

@@ -56,7 +56,7 @@ qtcreator-*
 # Vim
 *.swp
 
-# Continous Integration
+# Continuous Integration
 .moveit_ci
 
 *.pyc

moveit_ros/moveit_servo/src/collision_monitor.cpp

@@ -66,7 +66,7 @@ void CollisionMonitor::start()
   }
   else
   {
-    RCLCPP_INFO_STREAM(LOGGER, "Collision monitor could not be started");
+    RCLCPP_ERROR_STREAM(LOGGER, "Collision monitor could not be started");
   }
 }
 

moveit_ros/moveit_servo/src/servo.cpp

@@ -401,7 +401,7 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
   else
   {
     servo_status_ = StatusCode::INVALID;
-    RCLCPP_WARN_STREAM(LOGGER, "SERVO : Incoming command type does not match expected command type.");
+    RCLCPP_WARN_STREAM(LOGGER, "Incoming servo command type does not match known command types.");
   }
 
   return joint_position_deltas;
@@ -477,7 +477,9 @@ KinematicState Servo::getNextJointState(const ServoInput& command)
     const double joint_limit_scale = jointLimitVelocityScalingFactor(target_joint_velocities, joint_bounds,
                                                                      servo_params_.override_velocity_scaling_factor);
     if (joint_limit_scale < 1.0)  // 1.0 means no scaling.
-      RCLCPP_WARN_STREAM(LOGGER, "Joint velocity limit scaling applied by a factor of " << joint_limit_scale);
+    {
+      RCLCPP_DEBUG_STREAM(LOGGER, "Joint velocity limit scaling applied by a factor of " << joint_limit_scale);
+    }
 
     target_joint_velocities *= joint_limit_scale;
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -252,7 +252,7 @@ std::optional<KinematicState> ServoNode::processTwistCommand()
     if (new_twist_msg_)
     {
       next_joint_state = result.second;
-      RCLCPP_INFO_STREAM(LOGGER, "Twist command timed out. Halting to a stop.");
+      RCLCPP_DEBUG_STREAM(LOGGER, "Twist command timed out. Halting to a stop.");
     }
   }
 

moveit_ros/moveit_servo/src/utils/command.cpp

@@ -198,7 +198,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
     if (!is_planning_frame)
     {
       RCLCPP_WARN_STREAM(LOGGER,
-                         "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame);
+                         "Command frame is: " << command.frame_id << ", expected: " << servo_params.planning_frame);
     }
   }
   return std::make_pair(status, joint_position_delta);

moveit_ros/warehouse/src/broadcast.cpp

@@ -64,8 +64,6 @@ static const std::string CONSTRAINTS_TOPIC = "constraints";
 
 static const std::string STATES_TOPIC = "robot_states";
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros.warehouse.broadcast");
-
 using namespace std::chrono_literals;
 
 int main(int argc, char** argv)
@@ -75,6 +73,7 @@ int main(int argc, char** argv)
   node_options.allow_undeclared_parameters(true);
   node_options.automatically_declare_parameters_from_overrides(true);
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("publish_warehouse_data", node_options);
+  const auto logger = node->get_logger();
 
   // time to wait in between publishing messages
   double delay = 0.001;
@@ -141,7 +140,7 @@ int main(int argc, char** argv)
       moveit_warehouse::PlanningSceneWithMetadata pswm;
       if (pss.getPlanningScene(pswm, scene_name))
       {
-        RCLCPP_INFO(LOGGER, "Publishing scene '%s'",
+        RCLCPP_INFO(logger, "Publishing scene '%s'",
                     pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str());
         pub_scene->publish(static_cast<const moveit_msgs::msg::PlanningScene&>(*pswm));
         executor.spin_once(0ns);
@@ -154,14 +153,14 @@ int main(int argc, char** argv)
           std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
           std::vector<std::string> query_names;
           pss.getPlanningQueries(planning_queries, query_names, pswm->name);
-          RCLCPP_INFO(LOGGER, "There are %d planning queries associated to the scene",
+          RCLCPP_INFO(logger, "There are %d planning queries associated to the scene",
                       static_cast<int>(planning_queries.size()));
           rclcpp::sleep_for(500ms);
           for (std::size_t i = 0; i < planning_queries.size(); ++i)
           {
             if (req)
             {
-              RCLCPP_INFO(LOGGER, "Publishing query '%s'", query_names[i].c_str());
+              RCLCPP_INFO(logger, "Publishing query '%s'", query_names[i].c_str());
               pub_req->publish(static_cast<const moveit_msgs::msg::MotionPlanRequest&>(*planning_queries[i]));
               executor.spin_once(0ns);
             }
@@ -195,7 +194,7 @@ int main(int argc, char** argv)
       moveit_warehouse::ConstraintsWithMetadata cwm;
       if (cs.getConstraints(cwm, cname))
       {
-        RCLCPP_INFO(LOGGER, "Publishing constraints '%s'",
+        RCLCPP_INFO(logger, "Publishing constraints '%s'",
                     cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str());
         pub_constr->publish(static_cast<const moveit_msgs::msg::Constraints&>(*cwm));
         executor.spin_once(0ns);
@@ -217,7 +216,7 @@ int main(int argc, char** argv)
       moveit_warehouse::RobotStateWithMetadata rswm;
       if (rs.getRobotState(rswm, rname))
       {
-        RCLCPP_INFO(LOGGER, "Publishing state '%s'",
+        RCLCPP_INFO(logger, "Publishing state '%s'",
                     rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str());
         pub_state->publish(static_cast<const moveit_msgs::msg::RobotState&>(*rswm));
         executor.spin_once(0ns);
@@ -227,7 +226,7 @@ int main(int argc, char** argv)
   }
 
   rclcpp::sleep_for(1s);
-  RCLCPP_INFO(LOGGER, "Done.");
+  RCLCPP_INFO(logger, "Done.");
 
   return 0;
 }