From cc7a0d422b294ffc7c01a52df074140bb3c5aa15 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Wed, 11 Oct 2023 17:37:45 -0400 Subject: [PATCH 1/5] Fix levels in servo logs (#2440) --- moveit_ros/moveit_servo/src/collision_monitor.cpp | 2 +- moveit_ros/moveit_servo/src/servo.cpp | 6 ++++-- moveit_ros/moveit_servo/src/servo_node.cpp | 2 +- moveit_ros/moveit_servo/src/utils/command.cpp | 2 +- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index 10090cfe10e..3e343110222 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/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"); } } diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index b1313193712..3beed00e2ea 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/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; diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index c2848919d6c..02dd61bce96 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -252,7 +252,7 @@ std::optional 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."); } } diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index e6e932eac57..9ddfbda8b00 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/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); From 6d2eaabc958830466e0b595640348ebc6226554d Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 12 Oct 2023 06:00:34 -0600 Subject: [PATCH 2/5] Use node logging in warehouse broadcast (#2432) --- moveit_ros/warehouse/src/broadcast.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index 24f7490be65..208713d589f 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/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(*pswm)); executor.spin_once(0ns); @@ -154,14 +153,14 @@ int main(int argc, char** argv) std::vector planning_queries; std::vector 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(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(*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(*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(*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; } From 1e4721a4ea9f8c998fefbf387997385bae8216c8 Mon Sep 17 00:00:00 2001 From: Ikko Eltociear Ashimine Date: Fri, 13 Oct 2023 01:30:53 +0900 Subject: [PATCH 3/5] Fix typo in .gitignore (#2446) Continous -> Continuous --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index fe1224a00fd..82f4adc9005 100644 --- a/.gitignore +++ b/.gitignore @@ -56,7 +56,7 @@ qtcreator-* # Vim *.swp -# Continous Integration +# Continuous Integration .moveit_ci *.pyc From 6108bff5a950878ccfbf1eb327c8853296e6bab9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 13 Oct 2023 08:59:57 -0600 Subject: [PATCH 4/5] CI job for building docker images for tutorial on main (#2449) Signed-off-by: Tyler Weaver --- .docker/tutorial-source/Dockerfile | 42 ++++++++++++ .github/workflows/tutorial_docker.yaml | 94 ++++++++++++++++++++++++++ 2 files changed, 136 insertions(+) create mode 100644 .docker/tutorial-source/Dockerfile create mode 100644 .github/workflows/tutorial_docker.yaml diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile new file mode 100644 index 00000000000..f3935aaaab0 --- /dev/null +++ b/.docker/tutorial-source/Dockerfile @@ -0,0 +1,42 @@ +# syntax = docker/dockerfile:1.3 + +# ghcr.io/ros-planning/moveit2:main-tutorial-${ROS_DISTRO} +# Source build of the repos file from the tutorail site + +ARG ROS_DISTRO=rolling +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 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 diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml new file mode 100644 index 00000000000..c6d5d6a7023 --- /dev/null +++ b/.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-tutorial-${{ matrix.ROS_DISTRO }}-${{ github.job }} + DH_IMAGE: moveit/moveit2:main-tutorial-${{ 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/buildkit-cache-dance@v2.1.2 + 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 From 7cfebe88e5ce9f7a68f0551ab92d187a67f2c621 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 13 Oct 2023 12:32:52 -0600 Subject: [PATCH 5/5] Rename tutorial docker (#2451) --- .docker/tutorial-source/Dockerfile | 2 +- .github/workflows/tutorial_docker.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile index f3935aaaab0..325949b2acc 100644 --- a/.docker/tutorial-source/Dockerfile +++ b/.docker/tutorial-source/Dockerfile @@ -1,6 +1,6 @@ # syntax = docker/dockerfile:1.3 -# ghcr.io/ros-planning/moveit2:main-tutorial-${ROS_DISTRO} +# ghcr.io/ros-planning/moveit2:main-${ROS_DISTRO}-tutorial-source # Source build of the repos file from the tutorail site ARG ROS_DISTRO=rolling diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index c6d5d6a7023..4eb21563975 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -21,8 +21,8 @@ jobs: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:main-tutorial-${{ matrix.ROS_DISTRO }}-${{ github.job }} - DH_IMAGE: moveit/moveit2:main-tutorial-${{ matrix.ROS_DISTRO }}-${{ github.job }} + 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: