From 79cd4f234f3c8d7463a5a95c2d2ad50e2f904635 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 10 Nov 2020 09:02:03 -0600 Subject: [PATCH] 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 | 87 +++++----- ur_robot_driver/config/ur5_controllers.yaml | 153 +++--------------- ur_robot_driver/hardware_interface_plugin.xml | 10 +- .../ur_robot_driver/dashboard_client_ros.h | 8 +- .../ur_robot_driver/hardware_interface.h | 52 +++++- .../ur_robot_driver/robot_state_helper.h | 9 +- ur_robot_driver/launch/ur5_bringup.launch.py | 31 ++++ ur_robot_driver/package.xml | 19 ++- ur_robot_driver/src/dashboard_client_ros.cpp | 4 +- ur_robot_driver/src/hardware_interface.cpp | 27 +++- ur_robot_driver/src/robot_state_helper.cpp | 4 +- .../src/robot_state_helper_node.cpp | 2 +- 12 files changed, 193 insertions(+), 213 deletions(-) create mode 100755 ur_robot_driver/launch/ur5_bringup.launch.py diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index dbff6b5f0..7b98dacc5 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/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() diff --git a/ur_robot_driver/config/ur5_controllers.yaml b/ur_robot_driver/config/ur5_controllers.yaml index e8a037fe0..2fd0d315f 100644 --- a/ur_robot_driver/config/ur5_controllers.yaml +++ b/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 diff --git a/ur_robot_driver/hardware_interface_plugin.xml b/ur_robot_driver/hardware_interface_plugin.xml index 4b83a21bd..ddd8914c3 100644 --- a/ur_robot_driver/hardware_interface_plugin.xml +++ b/ur_robot_driver/hardware_interface_plugin.xml @@ -1,7 +1,9 @@ - - + + - Universal Robots ROS Driver as plugin + ROS2 Control System Driver for the Universal Robots series. - \ No newline at end of file + diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h index 8bade3cf4..64f3f4413 100644 --- a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h +++ b/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 @@ -44,7 +43,7 @@ #include #include -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 diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index 583c62ee7..0f0d4bce7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/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 +#include + +// 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 joint_angle_commands_, current_joint_angles_; +}; -#endif // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED +} // namespace ur_robot_driver diff --git a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.h b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.h index 95dd5ffb6..eb94adaab 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.h +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.h @@ -24,8 +24,7 @@ * */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_ROS_ROBOT_STATE_HELPER_INCLUDED -#define UR_ROBOT_DRIVER_ROS_ROBOT_STATE_HELPER_INCLUDED +#pragma once #include #include @@ -33,7 +32,7 @@ #include #include -namespace ur_driver +namespace ur_robot_driver { /*! * \brief A small helper class around the robot state (constisting of 'robot_mode' and @@ -46,6 +45,4 @@ namespace ur_driver class RobotStateHelper { }; -} // namespace ur_driver - -#endif // ifndef UR_ROBOT_DRIVER_ROS_ROBOT_STATE_HELPER_INCLUDED +} // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur5_bringup.launch.py b/ur_robot_driver/launch/ur5_bringup.launch.py new file mode 100755 index 000000000..464a7500a --- /dev/null +++ b/ur_robot_driver/launch/ur5_bringup.launch.py @@ -0,0 +1,31 @@ +import os +import xacro +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + ld = LaunchDescription() + description_package_path = get_package_share_directory('ur_description') + robot_description_file = os.path.join(description_package_path, 'urdf', 'ur5.urdf.xacro') + + controller_package_path = get_package_share_directory('ur_robot_driver') + robot_controller_file = os.path.join(controller_package_path, 'config', 'ur5_controllers.yaml') + + with open(robot_description_file, 'r') as infile: + descr = infile.read() + robot_description = {'robot_description': descr} + + + return LaunchDescription([ + Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, robot_controller_file], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + ) + ]) \ No newline at end of file diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 463a0a08a..bc018ffcf 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -4,7 +4,13 @@ ur_robot_driver 0.0.0 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. + Thomas Timm Andersen + Simon Rasmussen + Felix Exner + Lea Steffen + Tristan Schnell Felix Exner + Apache-2.0 BSD-2-Clause Zlib @@ -13,21 +19,18 @@ https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues https://github.com/UniversalRobots/Universal_Robots_ROS_Driver - Thomas Timm Andersen - Simon Rasmussen - Felix Exner - Lea Steffen - Tristan Schnell - ament_cmake + hardware_interface + pluginlib + rclcpp ur_client_library ur_dashboard_msgs + controller_manager + ament_cmake - - diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp index f87429820..d1e7f85b9 100644 --- a/ur_robot_driver/src/dashboard_client_ros.cpp +++ b/ur_robot_driver/src/dashboard_client_ros.cpp @@ -27,6 +27,6 @@ #include -namespace ur_driver +namespace ur_robot_driver { -} // namespace ur_driver +} // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index e2d814cf5..8cb40cfd3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -19,13 +19,32 @@ //---------------------------------------------------------------------- /*!\file * - * \author Felix Exner exner@fzi.de - * \date 2019-04-11 + * \author Andy Zelenak zelenak@picknik.ai + * \date 2020-11-9 * */ //---------------------------------------------------------------------- #include -namespace ur_driver +namespace ur_robot_driver { -} // namespace ur_driver +hardware_interface::return_type URHardwareInterface::configure(const HardwareInfo& system_info) +{ + info_ = system_info; + status_ = status::CONFIGURED; + + current_joint_angles_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + joint_angle_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo& joint : info_.joints) + { + if (joint.type.compare("ros2_control_components/PositionJoint") != 0) + { + status_ = status::UNKNOWN; + return return_type::ERROR; + } + } + + return return_type::OK; +} +} // namespace ur_robot_driver diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index 7c56750e0..2314ac70a 100644 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -28,6 +28,6 @@ #include #include -namespace ur_driver +namespace ur_robot_driver { -} // namespace ur_driver +} // namespace ur_robot_driver diff --git a/ur_robot_driver/src/robot_state_helper_node.cpp b/ur_robot_driver/src/robot_state_helper_node.cpp index 1dfe06cf7..8a1a8ba60 100644 --- a/ur_robot_driver/src/robot_state_helper_node.cpp +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -27,7 +27,7 @@ #include -using namespace ur_driver; +using namespace ur_robot_driver; int main(int argc, char** argv) {