Add free drive mode support

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_{};
 

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<uint32_t>(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;