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 7a8e3deb6..1f89c1df2 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 freedriveCallback(const std_msgs::Float64Ptr& msg); std::unique_ptr ur_driver_; std::unique_ptr dashboard_client_; @@ -315,6 +316,7 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer set_io_srv_; ros::ServiceServer resend_robot_program_srv_; ros::Subscriber command_sub_; + ros::Subscriber freedrive_sub_; 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 94f275af3..531431819 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -319,6 +319,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw // set_digital_out(0, True) // end command_sub_ = robot_hw_nh.subscribe("script_command", 1, &HardwareInterface::commandCallback, this); + freedrive_sub_ = robot_hw_nh.subscribe("enable_freedrive_mode", 1, &HardwareInterface::freedriveCallback, this); // Names of the joints. Usually, this is given in the controller config file. if (!robot_hw_nh.getParam("joints", joint_names_)) @@ -1138,6 +1139,39 @@ end return true; } +void HardwareInterface::freedriveCallback(const std_msgs::Float64Ptr& msg) +{ + std::stringstream freedrive_script; + freedrive_script << "def freedriveProgram():\n"; + freedrive_script << "\t freedrive_mode()\n"; + if (msg->data == 0.0) + { + freedrive_script << "\t sleep(3.0)\n"; + freedrive_script << "end\n"; + } + else + { + freedrive_script << "\t sleep(" << msg->data << ")\n"; + freedrive_script << "\t end_freedrive_mode()\n"; + freedrive_script << "end\n"; + } + + 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."); + } + + if (ur_driver_->sendScript(freedrive_script.str())) + { + ROS_WARN_STREAM("Sent freedrive mode script to robot - you will have to restart the External Control program."); + } + else + { + ROS_ERROR_STREAM("Error sending freedrive mode script to robot"); + } +} + void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) { std::string str = msg->data;