From 1d1250411657fb2aefe6ec7f1f92c42671ee24c0 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 15 Jun 2021 20:02:02 +0000 Subject: [PATCH] Enable Rolling and Galactic CI (#494) * Test future versions of ROS2 * Add CI badges to README * Apply compatibility fixes Co-authored-by: Vatan Aksoy Tezer Co-authored-by: Tyler Weaver Co-authored-by: Henning Kayser --- .../workflows/build_and_test_galactic.yaml | 74 +++++++++++++++++++ .github/workflows/build_and_test_rolling.yaml | 74 +++++++++++++++++++ README.md | 3 +- moveit2_galactic.repos | 49 ++++++++++++ moveit2_rolling.repos | 41 ++++++++++ moveit_core/CMakeLists.txt | 1 + .../src/model_based_planning_context.cpp | 2 +- .../gripper_controller_handle.h | 8 +- ...low_joint_trajectory_controller_handle.cpp | 19 +++-- moveit_ros/moveit_servo/src/servo_calcs.cpp | 2 +- .../src/kinematics_plugin_loader.cpp | 2 +- .../CMakeLists.txt | 2 + .../src/move_group_interface.cpp | 1 + .../moveit_cpp/src/moveit_cpp.cpp | 9 ++- .../src/robot_interaction.cpp | 9 ++- 15 files changed, 273 insertions(+), 23 deletions(-) create mode 100644 .github/workflows/build_and_test_galactic.yaml create mode 100644 .github/workflows/build_and_test_rolling.yaml create mode 100644 moveit2_galactic.repos create mode 100644 moveit2_rolling.repos diff --git a/.github/workflows/build_and_test_galactic.yaml b/.github/workflows/build_and_test_galactic.yaml new file mode 100644 index 0000000000..7c0a35546b --- /dev/null +++ b/.github/workflows/build_and_test_galactic.yaml @@ -0,0 +1,74 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: Build and Test (galactic) + +on: + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + industrial_ci: + strategy: + matrix: + env: + - ROS_DISTRO: galactic + ROS_REPO: main + - ROS_DISTRO: galactic + ROS_REPO: testing + env: + UPSTREAM_WORKSPACE: moveit2_galactic.repos + AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src + TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release + CCACHE_DIR: ${{ github.workspace }}/.ccache + BASEDIR: ${{ github.workspace }}/.work + CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} + + name: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: cache upstream_ws + uses: pat-s/always-upload-cache@v2.1.3 + with: + path: ${{ env.BASEDIR }}/upstream_ws + key: upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('moveit2_galactic.repos') }}-${{ github.run_id }} + restore-keys: | + upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('moveit2_galactic.repos') }} + # The target directory cache doesn't include the source directory because + # that comes from the checkout. See "prepare target_ws for cache" task below + - name: cache target_ws + uses: pat-s/always-upload-cache@v2.1.3 + with: + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + uses: pat-s/always-upload-cache@v2.1.3 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} + - name: industrial_ci + uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} + - name: upload test artifacts (on failure) + uses: actions/upload-artifact@v2 + if: failure() + with: + name: test-results + path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml + - name: prepare target_ws for cache + if: always() + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws diff --git a/.github/workflows/build_and_test_rolling.yaml b/.github/workflows/build_and_test_rolling.yaml new file mode 100644 index 0000000000..a447c5340e --- /dev/null +++ b/.github/workflows/build_and_test_rolling.yaml @@ -0,0 +1,74 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: Build and Test (rolling) + +on: + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + industrial_ci: + strategy: + matrix: + env: + - ROS_DISTRO: rolling + ROS_REPO: main + - ROS_DISTRO: rolling + ROS_REPO: testing + env: + UPSTREAM_WORKSPACE: moveit2_rolling.repos + AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src + TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release + CCACHE_DIR: ${{ github.workspace }}/.ccache + BASEDIR: ${{ github.workspace }}/.work + CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} + + name: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: cache upstream_ws + uses: pat-s/always-upload-cache@v2.1.3 + with: + path: ${{ env.BASEDIR }}/upstream_ws + key: upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('moveit2_rolling.repos') }}-${{ github.run_id }} + restore-keys: | + upstream_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('moveit2_rolling.repos') }} + # The target directory cache doesn't include the source directory because + # that comes from the checkout. See "prepare target_ws for cache" task below + - name: cache target_ws + uses: pat-s/always-upload-cache@v2.1.3 + with: + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + uses: pat-s/always-upload-cache@v2.1.3 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} + - name: industrial_ci + uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} + - name: upload test artifacts (on failure) + uses: actions/upload-artifact@v2 + if: failure() + with: + name: test-results + path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml + - name: prepare target_ws for cache + if: always() + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws diff --git a/README.md b/README.md index 1b0fb65688..39fd55910e 100644 --- a/README.md +++ b/README.md @@ -6,9 +6,10 @@ The MoveIt Motion Planning Framework for **ROS 2**. For ROS 1, see [MoveIt 1](ht ## Continuous Integration Status - [![Formatting (pre-commit)](https://github.com/ros-planning/moveit2/actions/workflows/format.yaml/badge.svg?branch=main)](https://github.com/ros-planning/moveit2/actions/workflows/format.yaml?query=branch%3Amain) [![Build and Test (Foxy)](https://github.com/ros-planning/moveit2/actions/workflows/build_and_test.yaml/badge.svg?branch=main)](https://github.com/ros-planning/moveit2/actions/workflows/build_and_test.yaml?query=branch%3Amain) +[![Build and Test (Rolling)](https://github.com/ros-planning/moveit2/actions/workflows/build_and_test_rolling.yaml/badge.svg?branch=main)](https://github.com/ros-planning/moveit2/actions/workflows/build_and_test_rolling.yaml?query=branch%3Amain) +[![Build and Test (Galactic)](https://github.com/ros-planning/moveit2/actions/workflows/build_and_test_galactic.yaml/badge.svg?branch=main)](https://github.com/ros-planning/moveit2/actions/workflows/build_and_test_galactic.yaml?query=branch%3Amain) [![Code Coverage](https://codecov.io/gh/ros-planning/moveit2/branch/main/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit2) ## General MoveIt Documentation diff --git a/moveit2_galactic.repos b/moveit2_galactic.repos new file mode 100644 index 0000000000..752be96563 --- /dev/null +++ b/moveit2_galactic.repos @@ -0,0 +1,49 @@ +repositories: + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs + version: galactic-devel + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox + version: ros2-master + geometric_shapes: + type: git + url: https://github.com/ros-planning/geometric_shapes + version: ros2 + moveit_msgs: + type: git + url: https://github.com/ros-planning/moveit_msgs + version: ros2 + moveit_resources: + type: git + url: https://github.com/ros-planning/moveit_resources + version: ros2 + object_recognition_msgs: + type: git + url: https://github.com/wg-perception/object_recognition_msgs + version: ros2 + random_numbers: + type: git + url: https://github.com/ros-planning/random_numbers + version: ros2 + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control + version: master + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers + version: master + srdfdom: + type: git + url: https://github.com/ros-planning/srdfdom + version: ros2 + warehouse_ros: + type: git + url: https://github.com/ros-planning/warehouse_ros + version: ros2 + warehouse_ros_mongo: + type: git + url: https://github.com/ros-planning/warehouse_ros_mongo + version: ros2 diff --git a/moveit2_rolling.repos b/moveit2_rolling.repos new file mode 100644 index 0000000000..275d436c0f --- /dev/null +++ b/moveit2_rolling.repos @@ -0,0 +1,41 @@ +repositories: + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs + version: galactic-devel + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox + version: ros2-master + geometric_shapes: + type: git + url: https://github.com/ros-planning/geometric_shapes + version: ros2 + moveit_msgs: + type: git + url: https://github.com/ros-planning/moveit_msgs + version: ros2 + moveit_resources: + type: git + url: https://github.com/ros-planning/moveit_resources + version: ros2 + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control + version: master + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers + version: master + srdfdom: + type: git + url: https://github.com/ros-planning/srdfdom + version: ros2 + warehouse_ros: + type: git + url: https://github.com/ros-planning/warehouse_ros + version: ros2 + warehouse_ros_mongo: + type: git + url: https://github.com/ros-planning/warehouse_ros_mongo + version: ros2 diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 36d58c1829..1639813ad8 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -203,6 +203,7 @@ if(BUILD_TESTING) set(ament_cmake_cppcheck_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_lint_cmake_FOUND TRUE) # Run all lint tests in package.xml except those listed above ament_lint_auto_find_test_dependencies() diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 9ff759714a..f051f6385e 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -974,7 +974,7 @@ bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(con constraints_library_->loadConstraintApproximations(constraint_path); std::stringstream ss; constraints_library_->printConstraintApproximations(ss); - RCLCPP_INFO(LOGGER, ss.str()); + RCLCPP_INFO_STREAM(LOGGER, ss.str()); return true; } return false; diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index a9299aa574..98d5849b53 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -136,9 +136,9 @@ class GripperControllerHandle : public ActionBasedControllerHandle::SendGoalOptions send_goal_options; // Active callback - send_goal_options.goal_response_callback = [this](const auto& /* unused-arg */) { - RCLCPP_DEBUG_STREAM(LOGGER, name_ << " started execution"); - }; + send_goal_options.goal_response_callback = + [this](std::shared_future::GoalHandle::SharedPtr> + /* unused-arg */) { RCLCPP_DEBUG_STREAM(LOGGER, name_ << " started execution"); }; // Send goal auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options); current_goal_ = current_goal_future.get(); @@ -213,6 +213,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle command_joints_; -}; +}; // namespace moveit_simple_controller_manager } // end namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index a4aec33e04..6d7171ee14 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -63,14 +63,17 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::ms rclcpp_action::Client::SendGoalOptions send_goal_options; // Active callback - send_goal_options.goal_response_callback = [this](const auto& future) { - RCLCPP_INFO_STREAM(LOGGER, name_ << " started execution"); - const auto& goal_handle = future.get(); - if (!goal_handle) - RCLCPP_WARN(LOGGER, "Goal request rejected"); - else - RCLCPP_INFO(LOGGER, "Goal request accepted!"); - }; + send_goal_options.goal_response_callback = + [this]( + std::shared_future::GoalHandle::SharedPtr> + future) { + RCLCPP_INFO_STREAM(LOGGER, name_ << " started execution"); + const auto& goal_handle = future.get(); + if (!goal_handle) + RCLCPP_WARN(LOGGER, "Goal request rejected"); + else + RCLCPP_INFO(LOGGER, "Goal request accepted!"); + }; done_ = false; last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 8fb79a55b9..a7aeaaff5f 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -459,7 +459,7 @@ rcl_interfaces::msg::SetParametersResult ServoCalcs::robotLinkCommandFrameCallba rcl_interfaces::msg::SetParametersResult result; result.successful = true; robot_link_command_frame_ = parameter.as_string(); - RCLCPP_INFO(LOGGER, "robot_link_command_frame changed to: " + robot_link_command_frame_); + RCLCPP_INFO_STREAM(LOGGER, "robot_link_command_frame changed to: " + robot_link_command_frame_); return result; }; diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index 46b672cfb5..cd3e587b3b 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -132,7 +132,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): "; for (const auto& tip : tips) tip_debug << tip << ", "; - RCLCPP_DEBUG(LOGGER, tip_debug.str()); + RCLCPP_DEBUG_STREAM(LOGGER, tip_debug.str()); return tips; } diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index baf0dca83e..4462e3bbc4 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -1,5 +1,7 @@ set(MOVEIT_LIB_NAME moveit_trajectory_execution_manager) +find_package(tinyxml_vendor REQUIRED) + add_library(${MOVEIT_LIB_NAME} SHARED src/trajectory_execution_manager.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${MOVEIT_LIB_NAME} diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 270258fd19..43eb45c912 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 71ddfa8df7..07321fff76 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -38,7 +38,8 @@ #include #include #include - +#include +#include #include #include #include @@ -67,7 +68,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) { const std::string error = "Unable to configure planning scene monitor"; - RCLCPP_FATAL(LOGGER, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } @@ -76,7 +77,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options { const std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; - RCLCPP_FATAL(LOGGER, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } @@ -84,7 +85,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options)) { const std::string error = "Failed to load planning pipelines from parameter server"; - RCLCPP_FATAL(LOGGER, error); + RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 64d8112765..bea4b74223 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -572,10 +572,13 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable) { std::string topic_name = int_marker_move_topics_[i]; std::string marker_name = int_marker_names_[i]; - int_marker_move_subscribers_.push_back(node_->create_subscription( - topic_name, 1, [this, marker_name](const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) { + std::function subscription_callback = + [this, marker_name](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { moveInteractiveMarker(marker_name, msg); - })); + }; + auto subscription = + node_->create_subscription(topic_name, 1, subscription_callback); + int_marker_move_subscribers_.push_back(subscription); } } }