Port MTC to ROS2

.github/workflows/ci.yaml

@@ -14,32 +14,36 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - IMAGE: melodic-source
+          # TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers
+          - IMAGE: galactic-source
             NAME: ccov
             TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
-          - IMAGE: master-source
+          - IMAGE: rolling-source
             CXXFLAGS: >-
-             -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
+             -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls
              -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter
-          - IMAGE: noetic-source
+          - IMAGE: rolling-source
             CXX: clang++
             CLANG_TIDY: true
             CXXFLAGS: >-
-             -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
+             -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls
              -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy
-          - IMAGE: noetic-source
+          # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
+          # see https://github.com/google/sanitizers/wiki/AddressSanitizer
+          - IMAGE: rolling-source
             NAME: asan
             DOCKER_RUN_OPTS: >-
               -e PRELOAD=libasan.so.5
-              -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
+              -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
             TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
 
     env:
-      CATKIN_LINT: true
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
-      DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
+      DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
       UNDERLAY: /root/ws_moveit/install
-      DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
+      # TODO: Port to ROS2
+      # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
+      UPSTREAM_WORKSPACE: .repos
       CCACHE_DIR: ${{ github.workspace }}/.ccache
       BASEDIR: ${{ github.workspace }}/.work
       CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
@@ -48,7 +52,7 @@ jobs:
       CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
       CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }}
 
-    name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
+    name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v2

.github/workflows/format.yaml

@@ -17,9 +17,6 @@ jobs:
       - uses: actions/setup-python@v2
       - name: Install clang-format-10
         run: sudo apt-get install clang-format-10
-      - uses: rhaschke/install-catkin_lint-action@v1.0
-        with:
-          distro: noetic
       - uses: pre-commit/action@v2.0.0
         id: precommit
       - name: Upload pre-commit changes

.pre-commit-config.yaml

@@ -42,10 +42,3 @@ repos:
         language: system
         files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
         args: ['-fallback-style=none', '-i']
-      - id: catkin_lint
-        name: catkin_lint
-        description: Check package.xml and cmake files
-        entry: catkin_lint .
-        language: system
-        always_run: true
-        pass_filenames: false

.repos

@@ -0,0 +1,5 @@
+repositories:
+  rosparam_shortcuts:
+    type: git
+    url: https://github.com/PickNikRobotics/rosparam_shortcuts
+    version: ros2

capabilities/CMakeLists.txt

@@ -1,40 +1,38 @@
-cmake_minimum_required(VERSION 3.1.3)
+cmake_minimum_required(VERSION 3.5)
 project(moveit_task_constructor_capabilities)
 
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++17)
 
-find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  moveit_core
-  moveit_ros_move_group
-  moveit_task_constructor_core
-  moveit_task_constructor_msgs
-  pluginlib
-  std_msgs
-)
-
-catkin_package(
-  LIBRARIES $PROJECT_NAME}
-  CATKIN_DEPENDS
-    actionlib
-    moveit_task_constructor_msgs
-    std_msgs
-)
+find_package(ament_cmake REQUIRED)
 
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
+find_package(Boost REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_move_group REQUIRED)
+find_package(moveit_task_constructor_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(std_msgs REQUIRED)
 
-add_library(${PROJECT_NAME}
+add_library(${PROJECT_NAME} SHARED
    src/execute_task_solution_capability.cpp
 )
-add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME}
+                          Boost
+                          rclcpp_action
+                          moveit_core
+                          moveit_ros_move_group
+                          moveit_task_constructor_msgs
+)
 
 install(TARGETS ${PROJECT_NAME}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION lib
 )
 
-install(FILES capabilities_plugin_description.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
+pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
+ament_export_dependencies(rclcpp_action)
+ament_export_dependencies(moveit_core)
+ament_export_dependencies(moveit_ros_move_group)
+ament_export_dependencies(moveit_task_constructor_msgs)
+ament_export_dependencies(pluginlib)
+ament_export_dependencies(std_msgs)
+ament_package()

capabilities/capabilities_plugin_description.xml

@@ -1,4 +1,4 @@
-<library path="libmoveit_task_constructor_capabilities">
+<library path="moveit_task_constructor_capabilities">
   <class name="move_group/ExecuteTaskSolutionCapability" type="move_group::ExecuteTaskSolutionCapability" base_class_type="move_group::MoveGroupCapability">
     <description>
       Action server to execute solutions generated through the MoveIt Task Constructor.

capabilities/package.xml

@@ -10,17 +10,18 @@
 
 	<license>BSD</license>
 
-	<buildtool_depend>catkin</buildtool_depend>
+	<buildtool_depend>ament_cmake</buildtool_depend>
 
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_move_group</depend>
-	<depend>actionlib</depend>
+	<depend>moveit_ros_planning</depend>
+	<depend>rclcpp_action</depend>
 	<depend>pluginlib</depend>
 	<depend>std_msgs</depend>
 	<depend>moveit_task_constructor_core</depend>
 	<depend>moveit_task_constructor_msgs</depend>
 
 	<export>
-		<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
+		<build_type>ament_cmake</build_type>
 	</export>
 </package>

capabilities/src/execute_task_solution_capability.cpp

@@ -36,16 +36,16 @@
 
 #include "execute_task_solution_capability.h"
 
-#include <moveit/task_constructor/moveit_compat.h>
-
+#include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit/plan_execution/plan_execution.h>
 #include <moveit/trajectory_processing/trajectory_tools.h>
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/move_group/capability_names.h>
 #include <moveit/robot_state/conversions.h>
-#if MOVEIT_HAS_MESSAGE_CHECKS
 #include <moveit/utils/message_checks.h>
-#endif
+#include <moveit/moveit_cpp/moveit_cpp.h>
+
+#include <boost/algorithm/string/join.hpp>
 
 namespace {
 
@@ -80,66 +80,71 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
 }
 }  // namespace
 
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
+
 namespace move_group {
 
 ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}
 
 void ExecuteTaskSolutionCapability::initialize() {
 	// configure the action server
-	as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
-	    root_node_handle_, "execute_task_solution",
-	    std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
-	as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
-	as_->start();
+	as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
+	    context_->moveit_cpp_->getNode(), "execute_task_solution",
+	    ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
+	                                             std::placeholders::_1, std::placeholders::_2)),
+	    ActionServerType::CancelCallback(
+	        std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
+	    ActionServerType::AcceptedCallback(
+	        std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
 }
 
 void ExecuteTaskSolutionCapability::goalCallback(
-    const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
-	moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
+	auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();
 
+	const auto& goal = goal_handle->get_goal();
 	if (!context_->plan_execution_) {
-		const std::string response = "Cannot execute solution. ~allow_trajectory_execution was set to false";
-		result.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
-		as_->setAborted(result, response);
+		result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
+		goal_handle->abort(result);
 		return;
 	}
 
 	plan_execution::ExecutableMotionPlan plan;
 	if (!constructMotionPlan(goal->solution, plan))
-		result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
+		result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
 	else {
-		ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
-		result.error_code = context_->plan_execution_->executeAndMonitor(plan);
+		RCLCPP_INFO(LOGGER, "Executing TaskSolution");
+		result->error_code = context_->plan_execution_->executeAndMonitor(plan);
 	}
 
-	const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code);
-
-	if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
-		as_->setSucceeded(result, response);
-	else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
-		as_->setPreempted(result, response);
+	if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+		goal_handle->succeed(result);
+	else if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
+		goal_handle->canceled(result);
 	else
-		as_->setAborted(result, response);
+		goal_handle->abort(result);
 }
 
-void ExecuteTaskSolutionCapability::preemptCallback() {
+rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
 	if (context_->plan_execution_)
 		context_->plan_execution_->stop();
+	return rclcpp_action::CancelResponse::ACCEPT;
 }
 
-bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
+bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
                                                         plan_execution::ExecutableMotionPlan& plan) {
-	robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
+	moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
 
-	robot_state::RobotState state(model);
+	moveit::core::RobotState state(model);
 	{
 		planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
 		state = scene->getCurrentState();
 	}
 
 	plan.plan_components_.reserve(solution.sub_trajectory.size());
 	for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) {
-		const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory[i];
+		const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i];
 
 		plan.plan_components_.emplace_back();
 		plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
@@ -156,12 +161,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 			if (!joint_names.empty()) {
 				group = findJointModelGroup(*model, joint_names);
 				if (!group) {
-					ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {"
-					                                                  << boost::algorithm::join(joint_names, ", ") << "}");
+					RCLCPP_ERROR_STREAM(LOGGER, "Could not find JointModelGroup that actuates {"
+					                                << boost::algorithm::join(joint_names, ", ") << "}");
 					return false;
 				}
-				ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution",
-				                group->getName().c_str());
+				RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str());
 			}
 		}
 		exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
@@ -170,25 +174,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		/* TODO add action feedback and markers */
 		exec_traj.effect_on_success_ = [this, sub_traj,
 		                                description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
-#if MOVEIT_HAS_MESSAGE_CHECKS
 			if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
-#else
-			if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
-#endif
-				ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
+				RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
 				return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
 			}
 			return true;
 		};
 
-#if MOVEIT_HAS_MESSAGE_CHECKS
 		if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
-#else
-		if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
-#endif
 		    !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
-			ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
-			                       "invalid intermediate robot state in scene diff of SubTrajectory " << description);
+			RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of SubTrajectory " << description);
 			return false;
 		}
 	}

capabilities/src/execute_task_solution_capability.h

@@ -42,9 +42,9 @@
 #pragma once
 
 #include <moveit/move_group/move_group_capability.h>
-#include <actionlib/server/simple_action_server.h>
+#include <rclcpp_action/rclcpp_action.hpp>
 
-#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
+#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
 
 #include <memory>
 
@@ -58,13 +58,23 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability
 	void initialize() override;
 
 private:
-	bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
+	using ExecuteTaskSolutionAction = moveit_task_constructor_msgs::action::ExecuteTaskSolution;
+	using ActionServerType = rclcpp_action::Server<ExecuteTaskSolutionAction>;
+	bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
 	                         plan_execution::ExecutableMotionPlan& plan);
 
-	void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
-	void preemptCallback();
+	void goalCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle);
 
-	std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_;
+	rclcpp_action::CancelResponse
+	preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle);
+
+	/** Always accept the goal */
+	rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& uuid,
+	                                          const ExecuteTaskSolutionAction::Goal::ConstSharedPtr goal) const {
+		return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+	}
+
+	ActionServerType::SharedPtr as_;
 };
 
 }  // namespace move_group