Port moveit_simple_controller_manager to ROS 2 (moveit#158)

moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h

@@ -183,7 +183,7 @@ class MoveItControllerManager
   {
   }
 
-  virtual void initialize(std::shared_ptr<rclcpp::Node>& node) = 0;
+  virtual void initialize(const rclcpp::Node::SharedPtr& node) = 0;
 
   /** \brief Return a given named controller. */
   virtual MoveItControllerHandlePtr getControllerHandle(const std::string& name) = 0;

moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.1.3)
+cmake_minimum_required(VERSION 3.10.2)
 project(moveit_simple_controller_manager)
 
 set(CMAKE_CXX_STANDARD 14)
@@ -9,38 +9,43 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
 endif()
 
 find_package(Boost REQUIRED thread)
+find_package(ament_cmake REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
 
-find_package(catkin COMPONENTS
-  moveit_core
-  pluginlib
-  actionlib
-  roscpp
-  control_msgs
-  REQUIRED)
-
-include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
-
-catkin_package(
-  LIBRARIES ${PROJECT_NAME}
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS moveit_core actionlib control_msgs
-)
+include_directories(include)
 
 add_library(${PROJECT_NAME}
   src/moveit_simple_controller_manager.cpp
   src/follow_joint_trajectory_controller_handle.cpp
 )
 set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  control_msgs
+  pluginlib
+  moveit_core
+  rclcpp_action
+  Boost
+)
 
 install(TARGETS ${PROJECT_NAME}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
-
-install(FILES moveit_simple_controller_manager_plugin_description.xml
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-       )
-install(DIRECTORY include/${PROJECT_NAME}/
-       DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-       )
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+
+install(DIRECTORY include/ DESTINATION include)
+
+pluginlib_export_plugin_description_file(moveit_core moveit_simple_controller_manager_plugin_description.xml)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_include_directories(include)
+ament_export_dependencies(pluginlib)
+ament_export_dependencies(moveit_core)
+ament_export_dependencies(rclcpp_action)
+ament_export_dependencies(control_msgs)
+ament_export_dependencies(Boost)
+ament_package()

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h

@@ -37,7 +37,8 @@
 
 #pragma once
 
-#include <actionlib/client/simple_action_client.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
 #include <moveit/controller_manager/controller_manager.h>
 #include <moveit/macros/class_forward.h>
 #include <memory>
@@ -50,15 +51,20 @@ namespace moveit_simple_controller_manager
 class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveItControllerHandle
 {
 public:
-  ActionBasedControllerHandleBase(const std::string& name) : moveit_controller_manager::MoveItControllerHandle(name)
+  ActionBasedControllerHandleBase(const std::string& name, const std::string& logger_name)
+    : moveit_controller_manager::MoveItControllerHandle(name), LOGGER(rclcpp::get_logger(logger_name))
   {
   }
 
   virtual void addJoint(const std::string& name) = 0;
   virtual void getJoints(std::vector<std::string>& joints) = 0;
-  virtual void configure(XmlRpc::XmlRpcValue& /* config */)
-  {
-  }
+  // TODO(JafarAbdi): Revise parameter lookup
+  //  virtual void configure(XmlRpc::XmlRpcValue& /* config */)
+  //  {
+  //  }
+
+protected:
+  const rclcpp::Logger LOGGER;
 };
 
 MOVEIT_CLASS_FORWARD(ActionBasedControllerHandleBase)
@@ -70,33 +76,34 @@ template <typename T>
 class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
 {
 public:
-  ActionBasedControllerHandle(const std::string& name, const std::string& ns)
-    : ActionBasedControllerHandleBase(name), nh_("~"), done_(true), namespace_(ns)
+  ActionBasedControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns,
+                              const std::string& logger_name)
+    : ActionBasedControllerHandleBase(name, logger_name), node_(node), done_(true), namespace_(ns)
   {
-    controller_action_client_.reset(new actionlib::SimpleActionClient<T>(getActionName(), true));
+    controller_action_client_ = rclcpp_action::create_client<T>(node_, getActionName());
     unsigned int attempts = 0;
     double timeout;
-    nh_.param("trajectory_execution/controller_connection_timeout", timeout, 15.0);
+    node_->get_parameter_or("trajectory_execution.controller_connection_timeout", timeout, 15.0);
 
     if (timeout == 0.0)
     {
-      while (ros::ok() && !controller_action_client_->waitForServer(ros::Duration(5.0)))
+      while (rclcpp::ok() && !controller_action_client_->wait_for_action_server(std::chrono::seconds(5)))
       {
-        ROS_WARN_STREAM_NAMED("ActionBasedController", "Waiting for " << getActionName() << " to come up");
-        ros::Duration(1).sleep();
+        RCLCPP_WARN_STREAM(LOGGER, "Waiting for " << getActionName() << " to come up");
       }
     }
     else
     {
-      while (ros::ok() && !controller_action_client_->waitForServer(ros::Duration(timeout / 3)) && ++attempts < 3)
+      while (rclcpp::ok() &&
+             !controller_action_client_->wait_for_action_server(std::chrono::duration<double>(timeout / 3)) &&
+             ++attempts < 3)
       {
-        ROS_WARN_STREAM_NAMED("ActionBasedController", "Waiting for " << getActionName() << " to come up");
-        ros::Duration(1).sleep();
+        RCLCPP_WARN_STREAM(LOGGER, "Waiting for " << getActionName() << " to come up");
       }
     }
-    if (!controller_action_client_->isServerConnected())
+    if (!controller_action_client_->action_server_is_ready())
     {
-      ROS_ERROR_STREAM_NAMED("ActionBasedController", "Action client not connected: " << getActionName());
+      RCLCPP_ERROR_STREAM(LOGGER, "Action client not connected: " << getActionName());
       controller_action_client_.reset();
     }
 
@@ -114,24 +121,25 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
       return false;
     if (!done_)
     {
-      ROS_INFO_STREAM_NAMED("ActionBasedController", "Cancelling execution for " << name_);
-      controller_action_client_->cancelGoal();
+      RCLCPP_INFO_STREAM(LOGGER, "Cancelling execution for " << name_);
+      auto cancel_result_future = controller_action_client_->async_cancel_goal(current_goal_);
+      if (rclcpp::spin_until_future_complete(node_, cancel_result_future) !=
+          rclcpp::executor::FutureReturnCode::SUCCESS)
+      {
+        RCLCPP_ERROR(LOGGER, "Failed to cancel goal");
+      }
       last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
       done_ = true;
     }
     return true;
   }
 
-  bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) override
+  bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(0)) override
   {
+    auto result_future = controller_action_client_->async_get_result(current_goal_);
     if (controller_action_client_ && !done_)
-      return controller_action_client_->waitForResult(timeout);
-#if 1  // TODO: remove when https://github.com/ros/actionlib/issues/155 is fixed
-    // workaround for actionlib issue: waitForResult() might return before our doneCB finished
-    ros::Time deadline = ros::Time::now() + ros::Duration(0.1);  // limit waiting to 0.1s
-    while (!done_ && ros::ok() && ros::Time::now() < deadline)   // Check the done_ flag explicitly,
-      ros::Duration(0.0001).sleep();                             // which is eventually set in finishControllerExecution
-#endif
+      return (rclcpp::spin_until_future_complete(node_, result_future, timeout.to_chrono<std::chrono::seconds>()) ==
+              rclcpp::executor::FutureReturnCode::SUCCESS);
     return true;
   }
 
@@ -151,7 +159,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
   }
 
 protected:
-  ros::NodeHandle nh_;
+  const rclcpp::Node::SharedPtr node_;
   std::string getActionName(void) const
   {
     if (namespace_.empty())
@@ -160,16 +168,17 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
       return name_ + "/" + namespace_;
   }
 
-  void finishControllerExecution(const actionlib::SimpleClientGoalState& state)
+  void finishControllerExecution(const rclcpp_action::ResultCode& state)
   {
-    ROS_DEBUG_STREAM_NAMED("ActionBasedController", "Controller " << name_ << " is done with state " << state.toString()
-                                                                  << ": " << state.getText());
-    if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
+    RCLCPP_DEBUG_STREAM(LOGGER, "Controller " << name_ << " is done with state " << static_cast<int8_t>(state));
+    if (state == rclcpp_action::ResultCode::SUCCEEDED)
       last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
-    else if (state == actionlib::SimpleClientGoalState::ABORTED)
+    else if (state == rclcpp_action::ResultCode::ABORTED)
       last_exec_ = moveit_controller_manager::ExecutionStatus::ABORTED;
-    else if (state == actionlib::SimpleClientGoalState::PREEMPTED)
+    else if (state == rclcpp_action::ResultCode::CANCELED)
       last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
+    else if (state == rclcpp_action::ResultCode::UNKNOWN)
+      last_exec_ = moveit_controller_manager::ExecutionStatus::UNKNOWN;
     else
       last_exec_ = moveit_controller_manager::ExecutionStatus::FAILED;
     done_ = true;
@@ -187,7 +196,9 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
   std::vector<std::string> joints_;
 
   /* action client */
-  std::shared_ptr<actionlib::SimpleActionClient<T> > controller_action_client_;
+  typename rclcpp_action::Client<T>::SharedPtr controller_action_client_;
+  /* Current goal that have been sent to the action server */
+  typename rclcpp_action::ClientGoalHandle<T>::SharedPtr current_goal_;
 };
 
 }  // end namespace moveit_simple_controller_manager

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h

@@ -38,7 +38,9 @@
 #pragma once
 
 #include <moveit_simple_controller_manager/action_based_controller_handle.h>
-#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+#include <control_msgs/msg/joint_tolerance.hpp>
+#include <functional>
 
 namespace moveit_simple_controller_manager
 {
@@ -47,32 +49,30 @@ namespace moveit_simple_controller_manager
  * or anything using a control_mgs/FollowJointTrajectoryAction.
  */
 class FollowJointTrajectoryControllerHandle
-    : public ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>
+    : public ActionBasedControllerHandle<control_msgs::action::FollowJointTrajectory>
 {
 public:
-  FollowJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns)
-    : ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>(name, action_ns)
+  FollowJointTrajectoryControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name,
+                                        const std::string& action_ns)
+    : ActionBasedControllerHandle<control_msgs::action::FollowJointTrajectory>(
+          node, name, action_ns, "moveit.simple_controller_manager.follow_joint_trajectory_controller_handle")
   {
   }
 
   bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override;
 
-  void configure(XmlRpc::XmlRpcValue& config) override;
+  // TODO(JafarAbdi): Revise parameter lookup
+  // void configure(XmlRpc::XmlRpcValue& config) override;
 
 protected:
-  void configure(XmlRpc::XmlRpcValue& config, const std::string& config_name,
-                 std::vector<control_msgs::JointTolerance>& tolerances);
-  static control_msgs::JointTolerance& getTolerance(std::vector<control_msgs::JointTolerance>& tolerances,
-                                                    const std::string& name);
+  static control_msgs::msg::JointTolerance& getTolerance(std::vector<control_msgs::msg::JointTolerance>& tolerances,
+                                                         const std::string& name);
 
-  void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
-                              const control_msgs::FollowJointTrajectoryResultConstPtr& result);
+  void controllerDoneCallback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult&
+          wrapped_result);
 
-  void controllerActiveCallback();
-
-  void controllerFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback);
-
-  control_msgs::FollowJointTrajectoryGoal goal_template_;
+  control_msgs::action::FollowJointTrajectory::Goal goal_template_;
 };
 
 }  // end namespace moveit_simple_controller_manager