From e4a503fe5f0466c77a6005d49294f17fc32cd3d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 8 Dec 2017 14:05:07 +0100 Subject: [PATCH] various improvements and fixes for use_ros_control=true (#6) * Find matching hardware_interface using the required type The name of the controller was used in order to find and start the matching hardware interface. In consequence this meant that one could only define one controller for each hardware interface. Now, the controller's required type of hardware interface is used to find and start the matching hardware interface. * separate read & update in controller consume is defined as read+update, but update does not include read in ros_control terminology. * Handle latency in pipeline loop The controllers need to update at a rate of *at least* 125Hz, but the wait_dequeue_timed call could in theory slow the loop down to 62.5Hz. The old ur_modern_driver worked around this problem by sending goals at 4*125Hz. This patch exploits the onTimeout method of a consumer to update with the specified frequency of the control loop, even if no new state message arrived after the previous command. * publish wrench w.r.t. tcp frame The messages had an empty frame_id before and could not be displayed in RViz * support ros_control in indigo --- CMakeLists.txt | 5 ++++ include/ur_modern_driver/pipeline.h | 25 ++++++------------- include/ur_modern_driver/ros/controller.h | 20 +++++++++------ .../ur_modern_driver/ros/hardware_interface.h | 4 +-- src/ros/controller.cpp | 25 ++++++++++++++----- src/ros/hardware_interface.cpp | 14 +++++------ src/ros_main.cpp | 5 +++- 7 files changed, 58 insertions(+), 40 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e27394..e37a820 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,11 @@ add_compile_options(-Wall) add_compile_options(-Wextra) add_compile_options(-Wno-unused-parameter) +# support indigo's ros_control - This can be removed upon EOL indigo +if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") + add_definitions(-DUR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) +endif() + ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) diff --git a/include/ur_modern_driver/pipeline.h b/include/ur_modern_driver/pipeline.h index e902ed3..ca7058a 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -126,7 +126,7 @@ private: { if (!queue_.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer owerflowed!"); + LOG_ERROR("Pipeline producer overflowed!"); } } @@ -141,27 +141,18 @@ private: { consumer_.setupConsumer(); unique_ptr product; - Time last_pkg = Clock::now(); - Time last_warn = last_pkg; while (running_) { - // 16000us timeout was chosen because we should - // roughly recieve messages at 125hz which is every - // 8ms so double it for some error margin - if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(16))) + // timeout was chosen because we should receive messages + // at roughly 125hz (every 8ms) and have to update + // the controllers (i.e. the consumer) with *at least* 125Hz + // So we update the consumer more frequently via onTimeout + if (!queue_.wait_dequeue_timed(product, std::chrono::milliseconds(8))) { - Time now = Clock::now(); - auto pkg_diff = now - last_pkg; - auto warn_diff = now - last_warn; - if (pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1)) - { - last_warn = now; - consumer_.onTimeout(); - } + consumer_.onTimeout(); continue; } - last_pkg = Clock::now(); if (!consumer_.consume(std::move(product))) break; } @@ -201,4 +192,4 @@ public: pThread_.join(); cThread_.join(); } -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index d99dc07..df5d6de 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -49,13 +49,13 @@ private: } void read(RTShared& state); - bool update(RTShared& state); + bool update(); bool write(); void reset(); public: ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, - double max_vel_change); + double max_vel_change, std::string tcp_link); virtual ~ROSController() { } @@ -66,20 +66,26 @@ public: virtual void setupConsumer(); virtual bool consume(RTState_V1_6__7& state) { - return update(state); + read(state); + return update(); } virtual bool consume(RTState_V1_8& state) { - return update(state); + read(state); + return update(); } virtual bool consume(RTState_V3_0__1& state) { - return update(state); + read(state); + return update(); } virtual bool consume(RTState_V3_2__3& state) { - return update(state); + read(state); + return update(); } + virtual void onTimeout(); + virtual void onRobotStateChange(RobotState state); -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 50e311b..a68a1e6 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -43,7 +43,7 @@ class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface std::array tcp_; public: - WrenchInterface(); + WrenchInterface(std::string tcp_link); void update(RTShared &packet); typedef hardware_interface::ForceTorqueSensorInterface parent_type; static const std::string INTERFACE_NAME; @@ -80,4 +80,4 @@ public: typedef hardware_interface::PositionJointInterface parent_type; static const std::string INTERFACE_NAME; -}; \ No newline at end of file +}; diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index 3e8b0f0..bc844c4 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -1,10 +1,10 @@ #include "ur_modern_driver/ros/controller.h" ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower, - std::vector& joint_names, double max_vel_change) + std::vector& joint_names, double max_vel_change, std::string tcp_link) : controller_(this, nh_) , joint_interface_(joint_names) - , wrench_interface_() + , wrench_interface_(tcp_link) , position_interface_(follower, joint_interface_, joint_names) , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) { @@ -33,7 +33,16 @@ void ROSController::doSwitch(const std::list for (auto const& ci : start_list) { - auto ait = available_interfaces_.find(ci.name); + std::string requested_interface(""); + +#if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) + requested_interface = ci.hardware_interface; +#else + if (!ci.claimed_resources.empty()) + requested_interface = ci.claimed_resources[0].hardware_interface; +#endif + + auto ait = available_interfaces_.find(requested_interface); if (ait == available_interfaces_.end()) continue; @@ -74,13 +83,12 @@ void ROSController::read(RTShared& packet) wrench_interface_.update(packet); } -bool ROSController::update(RTShared& state) +bool ROSController::update() { auto time = ros::Time::now(); auto diff = time - lastUpdate_; lastUpdate_ = time; - read(state); controller_.update(time, diff, !service_enabled_); // emergency stop and such should not kill the pipeline @@ -101,6 +109,11 @@ bool ROSController::update(RTShared& state) return write(); } +void ROSController::onTimeout() +{ + update(); +} + void ROSController::onRobotStateChange(RobotState state) { bool next = (state == RobotState::Running); @@ -109,4 +122,4 @@ void ROSController::onRobotStateChange(RobotState state) service_enabled_ = next; service_cooldown_ = 125; -} \ No newline at end of file +} diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index dc1ce9b..759eca5 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/hardware_interface.h" #include "ur_modern_driver/log.h" -const std::string JointInterface::INTERFACE_NAME = "joint_state_controller"; +const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface"; JointInterface::JointInterface(std::vector &joint_names) { for (size_t i = 0; i < 6; i++) @@ -18,10 +18,10 @@ void JointInterface::update(RTShared &packet) } -const std::string WrenchInterface::INTERFACE_NAME = "force_torque_sensor_controller"; -WrenchInterface::WrenchInterface() +const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface"; +WrenchInterface::WrenchInterface(std::string tcp_link) { - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3)); + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3)); } void WrenchInterface::update(RTShared &packet) @@ -30,7 +30,7 @@ void WrenchInterface::update(RTShared &packet) } -const std::string VelocityInterface::INTERFACE_NAME = "vel_based_pos_traj_controller"; +const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface"; VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names, double max_vel_change) : commander_(commander), max_vel_change_(max_vel_change) @@ -62,7 +62,7 @@ void VelocityInterface::reset() } } -const std::string PositionInterface::INTERFACE_NAME = "pos_based_pos_traj_controller"; +const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface"; PositionInterface::PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) @@ -87,4 +87,4 @@ void PositionInterface::start() void PositionInterface::stop() { follower_.stop(); -} \ No newline at end of file +} diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 8d74e3e..0818108 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -28,6 +28,7 @@ static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); +static const std::string TCP_LINK_ARG("~tcp_link"); static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); static const std::vector DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", @@ -43,6 +44,7 @@ public: std::string prefix; std::string base_frame; std::string tool_frame; + std::string tcp_link; std::vector joint_names; double max_acceleration; double max_velocity; @@ -65,6 +67,7 @@ bool parse_args(ProgArgs &args) ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "ee_link"); ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS); return true; } @@ -109,7 +112,7 @@ int main(int argc, char **argv) if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change); + controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); rt_vec.push_back(controller); services.push_back(controller); }