Hardware interface framework (#3)

* Updating ur_robot_driver include guards and namespaces

* Compile the ros_control plugin, inherit from components::SystemInterface

* Do away with intermediate SystemInterface class

* Implement configure()

* Add controller_manager launch file

* Cleanup and clang format

ur_robot_driver/CMakeLists.txt

@@ -12,59 +12,72 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
 find_package(ur_client_library REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 
 include_directories(
   include
-  ${Boost_INCLUDE_DIRS}
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  pluginlib
+  rclcpp
+  ur_client_library
   ur_dashboard_msgs
 )
 
 add_library(ur_robot_driver_plugin
+  SHARED
   src/dashboard_client_ros.cpp
   src/hardware_interface.cpp
 )
-target_link_libraries(ur_robot_driver_plugin ur_client_library::urcl)
-ament_target_dependencies(ur_robot_driver_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-add_executable(ur_robot_driver_node
-  src/dashboard_client_ros.cpp
-  src/hardware_interface.cpp
-  src/hardware_interface_node.cpp
-)
-target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_client_library::urcl)
-ament_target_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-add_executable(dashboard_client
-  src/dashboard_client_ros.cpp
-  src/dashboard_client_node.cpp
+target_link_libraries(
+  ur_robot_driver_plugin
+  ur_client_library::urcl
 )
-target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
-ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-add_executable(robot_state_helper
-  src/robot_state_helper.cpp
-  src/robot_state_helper_node.cpp
+target_include_directories(
+  ur_robot_driver_plugin
+  PRIVATE
+  include
 )
-target_link_libraries(robot_state_helper ${catkin_LIBRARIES} ur_client_library::urcl)
-ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-
-  add_rostest(test/driver.test)
-endif()
-
-
-install(TARGETS ur_robot_driver_plugin ur_robot_driver_node robot_state_helper dashboard_client
-  ARCHIVE DESTINATION lib}
-  LIBRARY DESTINATION lib}
-  RUNTIME DESTINATION bin}
+ament_target_dependencies(
+  ur_robot_driver_plugin
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )
+pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml)
+
+# add_executable(ur_robot_driver_node
+#   src/dashboard_client_ros.cpp
+#   src/hardware_interface.cpp
+#   src/hardware_interface_node.cpp
+# )
+# target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_client_library::urcl)
+# ament_target_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# add_executable(dashboard_client
+#   src/dashboard_client_ros.cpp
+#   src/dashboard_client_node.cpp
+# )
+# target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
+# ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# add_executable(robot_state_helper
+#   src/robot_state_helper.cpp
+#   src/robot_state_helper_node.cpp
+# )
+# target_link_libraries(robot_state_helper ${catkin_LIBRARIES} ur_client_library::urcl)
+# ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# install(TARGETS ur_robot_driver_plugin ur_robot_driver_node robot_state_helper dashboard_client
+#   ARCHIVE DESTINATION lib}
+#   LIBRARY DESTINATION lib}
+#   RUNTIME DESTINATION bin}
+# )
 
 install(PROGRAMS scripts/tool_communication
   DESTINATION bin
@@ -79,10 +92,6 @@ install(DIRECTORY include/
   DESTINATION include
 )
 
-install(FILES hardware_interface_plugin.xml
-  DESTINATION share/${PROJECT_NAME}
-)
-
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 ament_package()

ur_robot_driver/config/ur5_controllers.yaml

@@ -1,136 +1,19 @@
-# Settings for ros_control control loop
-hardware_control_loop:
-   loop_hz: &loop_hz 125
 
-# Settings for ros_control hardware interface
-ur_hardware_interface:
-   joints: &robot_joints
-     - shoulder_pan_joint
-     - shoulder_lift_joint
-     - elbow_joint
-     - wrist_1_joint
-     - wrist_2_joint
-     - wrist_3_joint
-
-# Publish all joint states ----------------------------------
-joint_state_controller:
-   type:         joint_state_controller/JointStateController
-   publish_rate: *loop_hz
-
-# Publish wrench ----------------------------------
-force_torque_sensor_controller:
-   type:         force_torque_sensor_controller/ForceTorqueSensorController
-   publish_rate: *loop_hz
-
-# Publish speed_scaling factor
-speed_scaling_state_controller:
-   type:         ur_controllers/SpeedScalingStateController
-   publish_rate: *loop_hz
-
-# Joint Trajectory Controller - position based -------------------------------
-# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-scaled_pos_joint_traj_controller:
-   type: position_controllers/ScaledJointTrajectoryController
-   joints: *robot_joints
-   constraints:
-      goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
-      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
-      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
-      elbow_joint: {trajectory: 0.2, goal: 0.1}
-      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
-      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
-      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
-   stop_trajectory_duration: 0.5
-   state_publish_rate: *loop_hz
-   action_monitor_rate: 20
-
-pos_joint_traj_controller:
-   type: position_controllers/JointTrajectoryController
-   joints: *robot_joints
-   constraints:
-      goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
-      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
-      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
-      elbow_joint: {trajectory: 0.2, goal: 0.1}
-      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
-      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
-      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
-   stop_trajectory_duration: 0.5
-   state_publish_rate: *loop_hz
-   action_monitor_rate: 20
-
-scaled_vel_joint_traj_controller:
-   type: velocity_controllers/ScaledJointTrajectoryController
-   joints: *robot_joints
-   constraints:
-      goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
-      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
-      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
-      elbow_joint: {trajectory: 0.1, goal: 0.1}
-      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
-      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
-      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
-   gains:
-      #!!These values have not been optimized!!
-      shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-   # Use a feedforward term to reduce the size of PID gains
-   velocity_ff:
-      shoulder_pan_joint: 1.0
-      shoulder_lift_joint: 1.0
-      elbow_joint: 1.0
-      wrist_1_joint: 1.0
-      wrist_2_joint: 1.0
-      wrist_3_joint: 1.0
-   stop_trajectory_duration: 0.5
-   state_publish_rate: *loop_hz
-   action_monitor_rate: 20
-
-vel_joint_traj_controller:
-   type: velocity_controllers/JointTrajectoryController
-   joints: *robot_joints
-   constraints:
-      goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
-      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
-      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
-      elbow_joint: {trajectory: 0.1, goal: 0.1}
-      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
-      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
-      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
-   gains:
-      #!!These values have not been optimized!!
-      shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-      wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-   # Use a feedforward term to reduce the size of PID gains
-   velocity_ff:
-      shoulder_pan_joint: 1.0
-      shoulder_lift_joint: 1.0
-      elbow_joint: 1.0
-      wrist_1_joint: 1.0
-      wrist_2_joint: 1.0
-      wrist_3_joint: 1.0
-   stop_trajectory_duration: 0.5
-   state_publish_rate: *loop_hz
-   action_monitor_rate: 20
-
-# Pass an array of joint velocities directly to the joints
-joint_group_vel_controller:
-   type: velocity_controllers/JointGroupVelocityController
-   joints: *robot_joints
-
-robot_status_controller:
-   type: industrial_robot_status_controller/IndustrialRobotStatusController
-   handle_name: industrial_robot_status_handle
-   publish_rate: 10
+control_manager:
+  ros__parameters:
+    controllers:
+      - joint_trajectory_controller_position
+
+    joint_trajectory_controller_position:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+forward_command_controller_position:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    interface_name: position

ur_robot_driver/hardware_interface_plugin.xml

@@ -1,7 +1,9 @@
-<library path="lib/libur_robot_driver_plugin">
-  <class name="ur_driver/HardwareInterface" type="ur_driver::HardwareInterface" base_class_type="hardware_interface::RobotHW">
+<library path="ur_robot_driver_plugin">
+  <class name="ur_robot_driver/URHardwareInterface"
+         type="ur_robot_driver::URHardwareInterface"
+         base_class_type="hardware_interface::RobotHardwareInterface">
     <description>
-      Universal Robots ROS Driver as plugin
+		ROS2 Control System Driver for the Universal Robots series.
     </description>
   </class>
-</library>
+</library>

ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h

@@ -25,8 +25,7 @@
  */
 //----------------------------------------------------------------------
 
-#ifndef UR_ROBOT_DRIVER_ROS_DASHBOARD_CLIENT_H_INCLUDED
-#define UR_ROBOT_DRIVER_ROS_DASHBOARD_CLIENT_H_INCLUDED
+#pragma once
 
 #include <regex>
 
@@ -44,7 +43,7 @@
 #include <ur_dashboard_msgs/srv/popup.hpp>
 #include <ur_dashboard_msgs/srv/raw_request.hpp>
 
-namespace ur_driver
+namespace ur_robot_driver
 {
 /*!
  * \brief ROS wrapper for UR's dashboard server access. Many (not necessarily all) dashboard
@@ -53,5 +52,4 @@ namespace ur_driver
 class DashboardClientROS
 {
 };
-}  // namespace ur_driver
-#endif  // ifndef UR_ROBOT_DRIVER_ROS_DASHBOARD_CLIENT_H_INCLUDED
+}  // namespace ur_robot_driver

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -24,20 +24,58 @@
  *
  */
 //----------------------------------------------------------------------
-#ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
-#define UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
+#pragma once
 
-namespace ur_driver
+// System
+#include <memory>
+#include <vector>
+
+// ros2_control hardware_interface
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/components/actuator.hpp"
+#include "hardware_interface/components/sensor.hpp"
+#include "hardware_interface/components/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
+#include "hardware_interface/visibility_control.h"
+
+// ROS
+#include "rclcpp/macros.hpp"
+
+using hardware_interface::HardwareInfo;
+using hardware_interface::return_type;
+using hardware_interface::status;
+using hardware_interface::components::Actuator;
+using hardware_interface::components::Sensor;
+
+namespace ur_robot_driver
 {
 /*!
  * \brief The HardwareInterface class handles the interface between the ROS system and the main
  * driver. It contains the read and write methods of the main control loop and registers various ROS
  * topics and services.
  */
-class HardwareInterface
+class URHardwareInterface : public hardware_interface::components::SystemInterface
 {
-};
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(URHardwareInterface);
 
-}  // namespace ur_driver
+  hardware_interface::return_type configure(const HardwareInfo& system_info) final;
+
+  status get_status() const final
+  {
+    return status_;
+  }
+
+  return_type start() final;
+  return_type stop() final;
+  return_type read() final;
+  return_type write() final;
+
+protected:
+  HardwareInfo info_;
+  status status_;
+  std::vector<double> joint_angle_commands_, current_joint_angles_;
+};
 
-#endif  // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
+}  // namespace ur_robot_driver