Ros2 port

robot_controllers_interface/CMakeLists.txt

@@ -1,66 +1,61 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(robot_controllers_interface)
 
-if (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(Boost REQUIRED system)
-find_package(catkin REQUIRED
-  COMPONENTS
-    actionlib
-    pluginlib
-    roscpp
-    robot_controllers_msgs
-)
-
-catkin_package(
-  INCLUDE_DIRS
-    include
-  CATKIN_DEPENDS
-    actionlib
-    pluginlib
-    roscpp
-    robot_controllers_msgs
-  DEPENDS
-    Boost
-  LIBRARIES
-    robot_controllers_interface
-)
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(robot_controllers_msgs REQUIRED)
 
 include_directories(
   include
-  ${Boost_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
 )
 
-add_library(robot_controllers_interface
+set(library_name ${PROJECT_NAME})
+
+add_library(${library_name}
   src/controller_loader.cpp
   src/controller_manager.cpp
 )
-target_link_libraries(robot_controllers_interface
-  ${Boost_LIBRARIES}
-  ${catkin_LIBRARIES}
-)
-add_dependencies(robot_controllers_interface
-  robot_controllers_msgs_generate_messages_cpp
+
+set(dependencies
+  pluginlib
+  rclcpp
+  rclcpp_action
+  robot_controllers_msgs
 )
 
-install(
-  DIRECTORY include/
-  DESTINATION include
+ament_target_dependencies(${library_name}
+  ${dependencies}
 )
 
-install(
-  TARGETS robot_controllers_interface LIBRARY
-  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(TARGETS
+  ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY include/ DESTINATION include/)
+
 install(
   PROGRAMS
     scripts/get_controller_state.py
     scripts/start_controller.py
     scripts/stop_controller.py
   DESTINATION
-    ${CATKIN_PACKAGE_BIN_DESTINATION}
+    lib/${PROJECT_NAME}
 )
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+ament_package()

robot_controllers_interface/include/robot_controllers_interface/controller.h

@@ -33,9 +33,10 @@
 
 #include <string>
 #include <vector>
-#include <boost/shared_ptr.hpp>
-#include <ros/ros.h>
-#include <robot_controllers_interface/handle.h>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "robot_controllers_interface/handle.h"
 
 /**
  * \mainpage
@@ -85,7 +86,7 @@ class Controller : public Handle
    *        controller to get information about joints, etc.
    * @returns 0 if succesfully configured, negative values are error codes.
    */
-  virtual int init(ros::NodeHandle& nh, ControllerManager* manager)
+  virtual int init(rclcpp::Node& nh, ControllerManager* manager)
   {
     name_ = nh.getNamespace();
     // remove leading slash
@@ -123,7 +124,7 @@ class Controller : public Handle
    * @param time The system time.
    * @param dt The timestep since last call to update.
    */
-  virtual void update(const ros::Time& time, const ros::Duration& dt) = 0;
+  virtual void update(const rclcpp::Time& time, const rclcpp::Duration& dt) = 0;
 
   /** @brief Get the name of this controller */
   std::string getName()
@@ -147,8 +148,7 @@ class Controller : public Handle
   std::string name_;
 };
 
-// Some typedefs
-typedef boost::shared_ptr<Controller> ControllerPtr;
+using ControllerPtr = std::shared_ptr<Controller>;
 
 }  // namespace robot_controllers
 

robot_controllers_interface/include/robot_controllers_interface/controller_loader.h

@@ -32,9 +32,10 @@
 #define ROBOT_CONTROLLERS_INTERFACE_CONTROLLER_LOADER_H
 
 #include <string>
-#include <ros/ros.h>
-#include <pluginlib/class_loader.h>
-#include <robot_controllers_interface/controller.h>
+
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "robot_controllers_interface/controller.h"
 
 namespace robot_controllers
 {
@@ -62,7 +63,7 @@ class ControllerLoader
   bool reset();
 
   /** @brief If controller is active, calls through to controller. */
-  void update(const ros::Time& time, const ros::Duration& dt);
+  void update(const rclcpp::Time& time, const rclcpp::Duration& dt);
 
   /** @brief Returns true if the controller is active. */
   bool isActive();
@@ -76,7 +77,7 @@ class ControllerLoader
   bool active_;
 };
 
-typedef boost::shared_ptr<ControllerLoader> ControllerLoaderPtr;
+using ControllerLoaderPtr = std::shared_ptr<ControllerLoader>;
 
 }  // namespace robot_controllers
 

robot_controllers_interface/include/robot_controllers_interface/controller_manager.h

@@ -32,25 +32,28 @@
 #define ROBOT_CONTROLLERS_INTERFACE_CONTROLLER_MANAGER_H
 
 #include <string>
-#include <ros/ros.h>
-#include <actionlib/server/simple_action_server.h>
 
-#include <robot_controllers_msgs/QueryControllerStatesAction.h>
+#include "rclcpp/rclcpp.hpp"
 
-#include <robot_controllers_interface/joint_handle.h>
-#include <robot_controllers_interface/controller.h>
-#include <robot_controllers_interface/controller_loader.h>
+// #include <actionlib/server/simple_action_server.h>
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "robot_controllers_msgs/action/query_controller_states.hpp"
+
+#include "robot_controllers_interface/joint_handle.h"
+#include "robot_controllers_interface/controller.h"
+#include "robot_controllers_interface/controller_loader.h"
 
 namespace robot_controllers
 {
 
 /** @brief Base class for a controller manager. */
 class ControllerManager
 {
-  typedef actionlib::SimpleActionServer<robot_controllers_msgs::QueryControllerStatesAction> server_t;
+  using server_t = actionlib::SimpleActionServer<robot_controllers_msgs::QueryControllerStatesAction>;
 
-  typedef std::vector<ControllerLoaderPtr> ControllerList;
-  typedef std::vector<JointHandlePtr> JointHandleList;
+  using ControllerList = std::vector<ControllerLoaderPtr>;
+  using JointHandleList = std::vector<JointHandlePtr>;
 
 public:
   ControllerManager();
@@ -67,7 +70,7 @@ class ControllerManager
    *
    * Note: JointHandles should be added before this is called.
    */
-  virtual int init(ros::NodeHandle& nh);
+  virtual int init(rclcpp::Node& nh);
 
   /** @brief Start a controller. */
   virtual int requestStart(const std::string& name);
@@ -76,7 +79,7 @@ class ControllerManager
   virtual int requestStop(const std::string& name);
 
   /** @brief Update active controllers. */
-  virtual void update(const ros::Time& time, const ros::Duration& dt);
+  virtual void update(const rclcpp::Time& time, const rclcpp::Duration& dt);
 
   /** @brief Reset all controllers. */
   virtual void reset();
@@ -111,7 +114,7 @@ class ControllerManager
   ControllerList controllers_;
   JointHandleList joints_;
 
-  boost::shared_ptr<server_t> server_;
+  std::shared_ptr<server_t> server_;
 };
 
 }  // namespace robot_controllers

robot_controllers_interface/include/robot_controllers_interface/handle.h

@@ -32,7 +32,7 @@
 #define ROBOT_CONTROLLERS_INTERFACE_HANDLE_H
 
 #include <string>
-#include <boost/shared_ptr.hpp>
+#include <memory>
 
 namespace robot_controllers
 {
@@ -61,7 +61,7 @@ class Handle
   Handle& operator=(const Handle&);
 };
 
-typedef boost::shared_ptr<Handle> HandlePtr;
+using HandlePtr = std::shared_ptr<Handle>;
 
 }  // namespace robot_controllers
 

robot_controllers_interface/include/robot_controllers_interface/joint_handle.h

@@ -31,7 +31,7 @@
 #ifndef ROBOT_CONTROLLERS_INTERFACE_JOINT_HANDLE_H
 #define ROBOT_CONTROLLERS_INTERFACE_JOINT_HANDLE_H
 
-#include <robot_controllers_interface/handle.h>
+#include "robot_controllers_interface/handle.h"
 
 namespace robot_controllers
 {
@@ -109,7 +109,7 @@ class JointHandle : public Handle
   JointHandle& operator=(const JointHandle&);
 };
 
-typedef boost::shared_ptr<JointHandle> JointHandlePtr;
+using JointHandlePtr = std::shared_ptr<JointHandle>;
 
 }  // namespace robot_controllers
 

robot_controllers_interface/package.xml

@@ -1,4 +1,6 @@
-<package>
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>robot_controllers_interface</name>
   <version>0.6.0</version>
   <description>
@@ -11,16 +13,11 @@
 
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>actionlib</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>robot_controllers_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-
-  <run_depend>actionlib</run_depend>
-  <run_depend>pluginlib</run_depend>
-  <run_depend>robot_controllers_msgs</run_depend>
-  <run_depend>roscpp</run_depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>robot_controllers_msgs</depend>
 
 </package>

robot_controllers_interface/src/controller_loader.cpp

@@ -28,7 +28,7 @@
 
 // Author: Michael Ferguson
 
-#include <robot_controllers_interface/controller_loader.h>
+#include "robot_controllers_interface/controller_loader.h"
 
 namespace robot_controllers
 {
@@ -41,7 +41,7 @@ ControllerLoader::ControllerLoader() :
 
 bool ControllerLoader::init(const std::string& name, ControllerManager* manager)
 {
-  ros::NodeHandle nh(name);
+  rclcpp::Node nh(name);
   std::string controller_type;
 
   if (nh.getParam("type", controller_type))
@@ -54,12 +54,12 @@ bool ControllerLoader::init(const std::string& name, ControllerManager* manager)
     }
     catch (pluginlib::LibraryLoadException e)
     {
-      ROS_ERROR_STREAM("Plugin error while loading controller: " << e.what());
+      RCLCPP_ERROR(nh->get_logger(), "Plugin error while loading controller: %s", e.what());
       return false;
     }
     return true;
   }
-  ROS_ERROR_STREAM("Unable to load controller " << name.c_str());
+  RCLCPP_ERROR(nh->get_logger(), "Unable to load controller %s", name.c_str());
   return false;
 }
 
@@ -93,7 +93,7 @@ bool ControllerLoader::isActive()
   return active_;
 }
 
-void ControllerLoader::update(const ros::Time& time, const ros::Duration& dt)
+void ControllerLoader::update(const rclcpp::Time& time, const rclcpp::Duration& dt)
 {
   if (active_)
   {