From eb02187f60f0506e7788c53b358cfa4155d3c4b1 Mon Sep 17 00:00:00 2001 From: schmid-jn Date: Wed, 31 Jul 2024 09:01:31 +0200 Subject: [PATCH] Add free drive mode support --- .../ur_robot_driver/hardware_interface.h | 7 +++++ ur_robot_driver/src/hardware_interface.cpp | 31 ++++++++++++++++++- 2 files changed, 37 insertions(+), 1 deletion(-) 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 5d435aaf3..410cd989a 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -213,6 +213,7 @@ class HardwareInterface : public hardware_interface::RobotHW bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); void commandCallback(const std_msgs::StringConstPtr& msg); + void freeDriveModeCallback(const std_msgs::BoolConstPtr& msg); bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res); bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res); @@ -318,6 +319,12 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer resend_robot_program_srv_; ros::Subscriber command_sub_; + ros::Subscriber free_drive_mode_sub_; + double free_drive_mode_timeout_; + bool free_drive_mode_requested_; + ros::Time free_drive_mode_latest_request_; + urcl::control::FreedriveControlMessage free_drive_mode_control_msg_; + industrial_robot_status_interface::RobotStatus robot_status_resource_{}; industrial_robot_status_interface::IndustrialRobotStatusInterface robot_status_interface_{}; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 63ecdbe2b..c7da69a93 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -333,6 +333,13 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw // end command_sub_ = robot_hw_nh.subscribe("script_command", 1, &HardwareInterface::commandCallback, this); + // Create free drive functionality + free_drive_mode_sub_ = robot_hw_nh.subscribe("free_drive_mode", 1, &HardwareInterface::freeDriveModeCallback, this); + // timeout in sec from last received free_drive msg until free drive mode is automatically disabled + free_drive_mode_timeout_ = robot_hw_nh.param("free_drive_mode_timeout", 1.0); + free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_STOP; + free_drive_mode_requested_ = false; + // Names of the joints. Usually, this is given in the controller config file. if (!robot_hw_nh.getParam("joints", joint_names_)) { @@ -686,7 +693,18 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period runtime_state_ == static_cast(rtde::RUNTIME_STATE::PAUSING)) && robot_program_running_ && (!non_blocking_read_ || packet_read_)) { - if (position_controller_running_) + if (free_drive_mode_requested_ || free_drive_mode_control_msg_ != urcl::control::FreedriveControlMessage::FREEDRIVE_STOP) { + if (ros::Time::now() > (free_drive_mode_latest_request_ + ros::Duration(free_drive_mode_timeout_)) || !free_drive_mode_requested_) { + free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_STOP; + free_drive_mode_requested_ = false; + } else if (free_drive_mode_control_msg_ == urcl::control::FreedriveControlMessage::FREEDRIVE_STOP) { + free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_START; + } else { + free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP; + } + ur_driver_->writeFreedriveControlMessage(free_drive_mode_control_msg_); + } + else if (position_controller_running_) { ur_driver_->writeJointCommand(joint_position_command_, urcl::comm::ControlMode::MODE_SERVOJ); } @@ -1199,6 +1217,17 @@ void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) } } +void HardwareInterface::freeDriveModeCallback(const std_msgs::BoolConstPtr& msg) +{ + if (ur_driver_ == nullptr) + { + throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, " + "please contact the package maintainer."); + } + free_drive_mode_latest_request_ = ros::Time::now(); + free_drive_mode_requested_ = msg->data; +} + bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res) { use_spline_interpolation_ = req.data;