diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e273943..e37a8208 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 e902ed3c..ca7058a8 100644 --- a/include/ur_modern_driver/pipeline.h +++ b/include/ur_modern_driver/pipeline.h @@ -126,7 +126,7 @@ class Pipeline { if (!queue_.try_enqueue(std::move(p))) { - LOG_ERROR("Pipeline producer owerflowed!"); + LOG_ERROR("Pipeline producer overflowed!"); } } @@ -141,27 +141,18 @@ class Pipeline { 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 @@ class Pipeline 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 d99dc075..df5d6de5 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -49,13 +49,13 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons } 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 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons 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 50e311b1..a68a1e6d 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 @@ class PositionInterface : public HardwareInterface, public hardware_interface::P 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 3e8b0f08..bc844c46 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 dc1ce9bf..759eca58 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 8d74e3e8..08181088 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 @@ struct ProgArgs 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); }