Added a service to hand back control to the robot

This way, Ros can  be a part of a larger program.

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -33,6 +33,7 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
 #include <std_msgs/Bool.h>
+#include <std_srvs/Trigger.h>
 #include <realtime_tools/realtime_publisher.h>
 #include "tf2_msgs/TFMessage.h"
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -96,8 +97,12 @@ protected:
 
   void checkCalibration(const std::string& checksum);
 
+  bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
+
   std::unique_ptr<UrDriver> ur_driver_;
 
+  ros::ServiceServer deactivate_srv_;
+
   hardware_interface::JointStateInterface js_interface_;
   ur_controllers::ScaledPositionJointInterface spj_interface_;
   hardware_interface::PositionJointInterface pj_interface_;

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -101,6 +101,14 @@ public:
    */
   bool writeKeepalive();
 
+  /*!
+   * \brief Sends a stop command to the socket interface which will signal the program running on
+   * the robot to no longer listen for commands sent from the remote pc.
+   *
+   * \returns True on successful write.
+   */
+  bool stopControl();
+
   void startWatchdog();
 
 private:

ur_rtde_driver/resources/servoj.urscript

@@ -71,6 +71,7 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
 
 thread_servo = run servoThread()
 keepalive = -2
+textmsg("External control active")
 params_mult = socket_read_binary_integer(1+6, "reverse_socket")
 keepalive = params_mult[1]
 while keepalive > 0:
@@ -78,12 +79,10 @@ while keepalive > 0:
   socket_send_line(1, "reverse_socket")
   params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
   if params_mult[0] > 0:
+    keepalive = params_mult[1]
     if params_mult[1] > 1:
-      keepalive = params_mult[1]
       q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
       set_servo_setpoint(q)
-    else:
-      keepalive = 1
     end
   else:
     keepalive = keepalive - 1
@@ -94,5 +93,5 @@ end
 textmsg("Stopping communication and servoing")
 cmd_servo_state = SERVO_STOPPED
 sleep(.1)
-socket_close()
+socket_close("reverse_socket")
 kill thread_servo

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -193,6 +193,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
 
   tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
 
+  deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
+
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
 
   return true;
@@ -287,17 +289,22 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
 
 void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
 {
-  if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
-      runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING))
+  if ((runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
+       runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING)) &&
+      robot_program_running_)
   {
     if (position_controller_running_)
     {
       ur_driver_->writeJointCommand(joint_position_command_);
     }
-    else
+    else if (robot_program_running_)
     {
       ur_driver_->writeKeepalive();
     }
+    else
+    {
+      ur_driver_->stopControl();
+    }
   }
 }
 
@@ -448,4 +455,20 @@ void HardwareInterface ::checkCalibration(const std::string& checksum)
   }
   ROS_DEBUG_STREAM("Got calibration information from robot.");
 }
+
+bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
+{
+  if (isRobotProgramRunning())
+  {
+    robot_program_running_ = false;
+    res.success = true;
+    res.message = "Deactivated control";
+  }
+  else
+  {
+    res.success = true;
+    res.message = "No control active. Nothing to do.";
+  }
+  return true;
+}
 }  // namespace ur_driver

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -153,6 +153,16 @@ bool UrDriver::writeKeepalive()
   return false;
 }
 
+bool UrDriver::stopControl()
+{
+  if (reverse_interface_active_)
+  {
+    vector6d_t* fake = nullptr;
+    return reverse_interface_->write(fake, 0);
+  }
+  return false;
+}
+
 void UrDriver::startWatchdog()
 {
   handle_program_state_(false);