Implemented ROS topic to enable the freedrive mode.

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<urcl::UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> 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_{};

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;