diff --git a/ur_robot_driver/doc/features.md b/ur_robot_driver/doc/features.md index ed1de33a6..f41e4b848 100644 --- a/ur_robot_driver/doc/features.md +++ b/ur_robot_driver/doc/features.md @@ -24,6 +24,7 @@ | send custom script commands to robot | yes | | ROS 2 support | (yes)3 | | Reconnect on a disconnected robot | yes | +| activate and deactivate freedrive mode (with axis constraints) | yes | 1 Requires URCap (included in [resources](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/tree/master/ur_robot_driver/resources)): 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..240bbf2d1 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::StringPtr& 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_{}; @@ -333,6 +335,7 @@ class HardwareInterface : public hardware_interface::RobotHW std::string tcp_link_; bool robot_program_running_; ros::Publisher program_state_pub_; + ros::Publisher status_pub; bool controller_reset_necessary_; bool controllers_initialized_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3491644e6..1f65648b3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -37,6 +37,9 @@ #include #include +#include "ros/ros.h" +#include "std_msgs/String.h" + #include #include @@ -178,6 +181,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw // message gets published here. So this is equivalent to the information whether the robot accepts // commands from ROS side. program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); + status_pub = robot_hw_nh.advertise("/overseer/ur_hw_status", 5); tcp_transform_.header.frame_id = tf_prefix_ + "base"; tcp_transform_.child_frame_id = tf_prefix_ + "tool0_controller"; @@ -323,7 +327,8 @@ 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_)) { @@ -679,6 +684,9 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) if (!non_blocking_read_) { ROS_ERROR("Could not get fresh data package from robot"); + std_msgs::String str; + str.data = "ERR404"; + status_pub.publish(str); } } } @@ -1141,6 +1149,46 @@ end return true; } +void HardwareInterface::freedriveCallback(const std_msgs::StringPtr& msg) +{ + // Recieve a std::String message with data = "[X,Y,Z,RX,RY,RZ]" + // where X,Y,Z,RX,RY,RZ are 0 or 1 depending on the axis that + // are unconstrained + std::stringstream freedrive_script; + freedrive_script << "def freedriveProgram():\n"; + + // Freedrive mode will remain active until either the program is + // returned to External Control OR if data = "[0,0,0,0,0,0]" + if (msg->data.compare("[0,0,0,0,0,0]") != 0) + { + freedrive_script << "\t freedrive_mode(freeAxes=" << msg->data << ")\n"; + freedrive_script << "\t while True:\n"; + freedrive_script << "\t\t sleep(3.0)\n"; + freedrive_script << "\t end\n"; + freedrive_script << "end\n"; + } + else + { + 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;