Enable Rolling and Galactic CI

.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

.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

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

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

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

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()

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;

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h

@@ -136,9 +136,9 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     }
     rclcpp_action::Client<control_msgs::action::GripperCommand>::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<rclcpp_action::Client<control_msgs::action::GripperCommand>::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<control_msgs:
    * joint is considered the command.
    */
   std::set<std::string> command_joints_;
-};
+};  // namespace moveit_simple_controller_manager
 
 }  // end namespace moveit_simple_controller_manager

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<control_msgs::action::FollowJointTrajectory>::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<rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::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;

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;
 };
 

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;
   }

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}

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -39,6 +39,7 @@
 #include <stdexcept>
 #include <sstream>
 #include <memory>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <moveit/warehouse/constraints_storage.h>
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/move_group/capability_names.h>

moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp

@@ -38,7 +38,8 @@
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit/planning_scene_monitor/current_state_monitor.h>
 #include <moveit/common_planning_interface_objects/common_objects.h>
-
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/msg/quaternion_stamped.hpp>
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <rclcpp/rclcpp.hpp>
@@ -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,15 +77,15 @@ 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);
   }
 
   bool load_planning_pipelines = true;
   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);
   }