Re-add urscript topic (#7)

* Re-added UR script - for custom UR Script execution

* Restarting the driver when robot closes the connection on script error.

The pipelines work in the way that if the connection is
is closed by the control PC, it will not be re-established. This
happens for example if you use the URScript topic and upload
script that does not compile. The robot will then close the
connection, the pipeline will close and any subsequent
uploads will fail and noone realises there is a problem.

While we could re-establish the connection, I think much better
solution is to shutdown the driver in such case. This is much more
resilient behaviour as it will clean up any inconsistent driver state.

We can utilise "respawn" feature of ROS launch and restart such
driver automatically (launch files are updated as part of that change).

On top of "production" stability, it allows for much nicer development
workflow - you can use URScript topic for development of new scripts
and have the driver restart every time you make mistake.
Without it, any mistake requires restarting the driver manually.

CMakeLists.txt

@@ -172,6 +172,7 @@ set(${PROJECT_NAME}_SOURCES
     src/ros/rt_publisher.cpp
     src/ros/service_stopper.cpp
     src/ros/trajectory_follower.cpp
+    src/ros/urscript_handler.cpp
     src/ur/stream.cpp
     src/ur/server.cpp
     src/ur/commander.cpp

README.md

@@ -22,7 +22,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
 
 * Besides this, the driver subscribes to two new topics:
 
-  * ~~*/ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.~~
+  * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.
 
   * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
 

include/ur_modern_driver/pipeline.h

@@ -99,6 +99,18 @@ public:
   virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0;
 };
 
+class INotifier
+{
+public:
+  virtual void started(std::string name)
+  {
+  }
+  virtual void stopped(std::string name)
+  {
+  }
+};
+
+
 template <typename T>
 class Pipeline
 {
@@ -107,6 +119,8 @@ private:
   typedef Clock::time_point Time;
   IProducer<T>& producer_;
   IConsumer<T>& consumer_;
+  std::string name_;
+  INotifier& notifier_;
   BlockingReaderWriterQueue<unique_ptr<T>> queue_;
   atomic<bool> running_;
   thread pThread_, cThread_;
@@ -126,15 +140,17 @@ private:
       {
         if (!queue_.try_enqueue(std::move(p)))
         {
-          LOG_ERROR("Pipeline producer overflowed!");
+          LOG_ERROR("Pipeline producer overflowed! <%s>",name_.c_str());
         }
       }
 
       products.clear();
     }
     producer_.teardownProducer();
-    LOG_DEBUG("Pipline producer ended");
+    LOG_DEBUG("Pipeline producer ended! <%s>", name_.c_str());
     consumer_.stopConsumer();
+    running_ = false;
+    notifier_.stopped(name_);
   }
 
   void run_consumer()
@@ -157,13 +173,15 @@ private:
         break;
     }
     consumer_.teardownConsumer();
-    LOG_DEBUG("Pipline consumer ended");
+    LOG_DEBUG("Pipeline consumer ended! <%s>", name_.c_str());
     producer_.stopProducer();
+    running_ = false;
+    notifier_.stopped(name_);
   }
 
 public:
-  Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
+  Pipeline(IProducer<T>& producer, IConsumer<T>& consumer, std::string name, INotifier& notifier)
-    : producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false }
+    : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
   {
   }
 
@@ -175,6 +193,7 @@ public:
     running_ = true;
     pThread_ = thread(&Pipeline::run_producer, this);
     cThread_ = thread(&Pipeline::run_consumer, this);
+    notifier_.started(name_);
   }
 
   void stop()
@@ -182,7 +201,7 @@ public:
     if (!running_)
       return;
 
-    LOG_DEBUG("Stopping pipeline");
+    LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str());
 
     consumer_.stopConsumer();
     producer_.stopProducer();
@@ -191,5 +210,6 @@ public:
 
     pThread_.join();
     cThread_.join();
+    notifier_.stopped(name_);
   }
 };

include/ur_modern_driver/ros/urscript_handler.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <string>
+
+#include "std_msgs/String.h"
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ros/service_stopper.h"
+#include "ur_modern_driver/ur/commander.h"
+
+class URScriptHandler: public Service
+{
+private:
+
+  ros::NodeHandle nh_;
+  URCommander &commander_;
+  ros::Subscriber urscript_sub_;
+  RobotState state_;
+
+public:
+  URScriptHandler(URCommander &commander);
+  void urscriptInterface(const std_msgs::String::ConstPtr& msg);
+  void onRobotStateChange(RobotState state);
+};

include/ur_modern_driver/ur/commander.h

@@ -10,7 +10,7 @@ private:
   URStream &stream_;
 
 protected:
-  bool write(std::string &s);
+  bool write(const std::string &s);
   void formatArray(std::ostringstream &out, std::array<double, 6> &values);
 
 public:
@@ -23,7 +23,7 @@ public:
   virtual bool setAnalogOut(uint8_t pin, double value) = 0;
 
   // shared
-  bool uploadProg(std::string &s);
+  bool uploadProg(const std::string &s);
   bool stopj(double a = 10.0);
   bool setToolVoltage(uint8_t voltage);
   bool setFlag(uint8_t pin, bool value);

launch/ur10_bringup.launch

@@ -14,6 +14,7 @@
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="10.0"/>
   <arg name="prefix" default="" />
+  <arg name="shutdown_on_disconnect" default="true" />
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -24,6 +25,7 @@
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 
 </launch>

launch/ur10_bringup_compatible.launch

@@ -13,6 +13,7 @@
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="10.0"/>
+  <arg name="shutdown_on_disconnect" default="true" />
 
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
@@ -25,6 +26,7 @@
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
     <arg name="servoj_time"  value="0.08" />
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 
 </launch>

launch/ur10_bringup_joint_limited.launch

@@ -13,6 +13,7 @@
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="10.0"/>
   <arg name="prefix" default="" />
+  <arg name="shutdown_on_disconnect" default="true" />
 
   <include file="$(find ur_modern_driver)/launch/ur10_bringup.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
@@ -20,5 +21,6 @@
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
     <arg name="prefix" value="$(arg prefix)" />
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 </launch>

launch/ur10_ros_control.launch

@@ -14,6 +14,7 @@
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
+  <arg name="shutdown_on_disconnect" default="true" />
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -31,6 +32,7 @@
     <param name="prefix" value="$(arg prefix)" />
     <param name="base_frame" type="str" value="$(arg base_frame)"/>
     <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
+    <param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
   </node>
 
   <!-- Load controller settings -->

launch/ur3_bringup.launch

@@ -14,6 +14,7 @@
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="3.0"/>
   <arg name="prefix" default="" />
+  <arg name="shutdown_on_disconnect" default="true" />
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur3_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -25,6 +26,7 @@
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
     <arg name="prefix" value="$(arg prefix)" />
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 
 </launch>

launch/ur3_bringup_joint_limited.launch

@@ -13,6 +13,7 @@
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="3.0"/>
   <arg name="prefix" default="" />
+  <arg name="shutdown_on_disconnect" default="true" />
 
   <include file="$(find ur_modern_driver)/launch/ur3_bringup.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
@@ -20,5 +21,6 @@
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
     <arg name="prefix" value="$(arg prefix)" />
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 </launch>

launch/ur3_ros_control.launch

@@ -14,6 +14,7 @@
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
+  <arg name="shutdown_on_disconnect" default="true" />
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur3_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -31,6 +32,7 @@
     <param name="prefix" value="$(arg prefix)" />
     <param name="base_frame" type="str" value="$(arg base_frame)"/>
     <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
+    <param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
   </node>
 
   <!-- Load controller settings -->

launch/ur5_bringup.launch

@@ -14,6 +14,7 @@
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
   <arg name="prefix" default="" />
+  <arg name="shutdown_on_disconnect" default="true" />
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur5_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -25,6 +26,7 @@
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 
 </launch>

launch/ur5_bringup_compatible.launch

@@ -13,6 +13,7 @@
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
+  <arg name="shutdown_on_disconnect" default="true" />
 
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur5_upload.launch">
@@ -25,6 +26,7 @@
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
     <arg name="servoj_time"  value="0.08" />
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 
 </launch>

launch/ur5_bringup_joint_limited.launch

@@ -13,6 +13,7 @@
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
   <arg name="prefix" default="" />
+  <arg name="shutdown_on_disconnect" default="true" />
 
   <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
     <arg name="robot_ip" value="$(arg robot_ip)"/>
@@ -20,5 +21,6 @@
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
     <arg name="prefix" value="$(arg prefix)" />
+    <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
   </include>
 </launch>

launch/ur5_ros_control.launch

@@ -14,6 +14,7 @@
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
+  <arg name="shutdown_on_disconnect" default="true" />
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur5_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -31,6 +32,7 @@
     <param name="prefix" value="$(arg prefix)" />
     <param name="base_frame" type="str" value="$(arg base_frame)"/>
     <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
+    <param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
   </node>
 
   <!-- Load controller settings -->

launch/ur_common.launch

@@ -15,6 +15,7 @@
   <arg name="servoj_time" default="0.008" />
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
+  <arg name="shutdown_on_disconnect" default="true" />
 
   <!-- require_activation defines when the service /ur_driver/robot_enable needs to be called. -->
   <arg name="require_activation" default="Never" /> <!-- Never, Always, OnStartup -->
@@ -25,7 +26,7 @@
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
   <!-- driver -->
-  <node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" output="screen">
+  <node name="ur_driver" pkg="ur_modern_driver" type="ur_driver" output="screen" respawn="">
   <!-- copy the specified IP address to be consistant with ROS-Industrial spec. -->
     <param name="prefix" type="str" value="$(arg prefix)" />
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)" />
@@ -36,5 +37,6 @@
     <param name="base_frame" type="str" value="$(arg base_frame)"/>
     <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
     <param name="require_activation" type="str" value="$(arg require_activation)" />
+    <param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
   </node>
 </launch>

src/ros/urscript_handler.cpp

@@ -0,0 +1,42 @@
+#include "ur_modern_driver/ros/urscript_handler.h"
+#include "ur_modern_driver/log.h"
+
+URScriptHandler::URScriptHandler(URCommander &commander)
+    : commander_(commander)
+    , state_(RobotState::Error)
+{
+    LOG_INFO("Initializing ur_driver/URScript subscriber");
+    urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
+    LOG_INFO("The ur_driver/URScript initialized");
+}
+
+void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
+{
+  LOG_INFO("Message received");
+  switch (state_)
+  {
+    case RobotState::Running:
+      if (!commander_.uploadProg(msg->data))
+      {
+        LOG_ERROR("Program upload failed!");
+      }
+      break;
+    case RobotState::EmergencyStopped:
+      LOG_ERROR("Robot is emergency stopped");
+      break;
+    case RobotState::ProtectiveStopped:
+      LOG_ERROR("Robot is protective stopped");
+      break;
+    case RobotState::Error:
+      LOG_ERROR("Robot is not ready, check robot_mode");
+      break;
+    default:
+      LOG_ERROR("Robot is in undefined state");
+      break;
+  }
+}
+
+void URScriptHandler::onRobotStateChange(RobotState state)
+{
+   state_ = state;
+}

src/ros_main.cpp

@@ -13,6 +13,7 @@
 #include "ur_modern_driver/ros/rt_publisher.h"
 #include "ur_modern_driver/ros/service_stopper.h"
 #include "ur_modern_driver/ros/trajectory_follower.h"
+#include "ur_modern_driver/ros/urscript_handler.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
 #include "ur_modern_driver/ur/messages.h"
@@ -30,6 +31,7 @@ static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
 static const std::string TCP_LINK_ARG("~tcp_link");
 static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
+static const std::string SHUTDOWN_ON_DISCONNECT_ARG("~shutdown_on_disconnect");
 
 static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
                                                          "wrist_1_joint",      "wrist_2_joint",       "wrist_3_joint" };
@@ -51,8 +53,34 @@ public:
   double max_vel_change;
   int32_t reverse_port;
   bool use_ros_control;
+  bool shutdown_on_disconnect;
 };
 
+class IgnorePipelineStoppedNotifier : public INotifier
+{
+public:
+    void started(std::string name){
+        LOG_INFO("Starting pipeline %s", name.c_str());
+    }
+    void stopped(std::string name){
+        LOG_INFO("Stopping pipeline %s", name.c_str());
+    }
+};
+
+class ShutdownOnPipelineStoppedNotifier : public INotifier
+{
+public:
+    void started(std::string name){
+        LOG_INFO("Starting pipeline %s", name.c_str());
+    }
+    void stopped(std::string name){
+        LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
+        ros::shutdown();
+        exit(1);
+    }
+};
+
+
 bool parse_args(ProgArgs &args)
 {
   if (!ros::param::get(IP_ADDR_ARG, args.host))
@@ -69,6 +97,7 @@ bool parse_args(ProgArgs &args)
   ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
   ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0");
   ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS);
+  ros::param::param(SHUTDOWN_ON_DISCONNECT_ARG, args.shutdown_on_disconnect, true);
   return true;
 }
 
@@ -107,6 +136,7 @@ int main(int argc, char **argv)
 
   TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
 
+  INotifier *notifier(nullptr);
   ROSController *controller(nullptr);
   ActionServer *action_server(nullptr);
   if (args.use_ros_control)
@@ -124,8 +154,21 @@ int main(int argc, char **argv)
     services.push_back(action_server);
   }
 
+  URScriptHandler urscript_handler(*rt_commander);
+  services.push_back(&urscript_handler);
+  if (args.shutdown_on_disconnect)
+  {
+    LOG_INFO("Notifier: Pipeline disconnect will shutdown the node");
+    notifier = new ShutdownOnPipelineStoppedNotifier();
+  }
+  else
+  {
+    LOG_INFO("Notifier: Pipeline disconnect will be ignored.");
+    notifier = new IgnorePipelineStoppedNotifier();
+  }
+
   MultiConsumer<RTPacket> rt_cons(rt_vec);
-  Pipeline<RTPacket> rt_pl(rt_prod, rt_cons);
+  Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
 
   // Message packets
   auto state_parser = factory.getStateParser();
@@ -137,7 +180,7 @@ int main(int argc, char **argv)
 
   vector<IConsumer<StatePacket> *> state_vec{ &state_pub, &service_stopper };
   MultiConsumer<StatePacket> state_cons(state_vec);
-  Pipeline<StatePacket> state_pl(state_prod, state_cons);
+  Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
 
   LOG_INFO("Starting main loop");
 

src/ur/commander.cpp

@@ -1,7 +1,7 @@
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/log.h"
 
-bool URCommander::write(std::string &s)
+bool URCommander::write(const std::string &s)
 {
   size_t len = s.size();
   const uint8_t *data = reinterpret_cast<const uint8_t *>(s.c_str());
@@ -20,8 +20,9 @@ void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &va
   out << "]";
 }
 
-bool URCommander::uploadProg(std::string &s)
+bool URCommander::uploadProg(const std::string &s)
 {
+  LOG_DEBUG("Sending program [%s]",s.c_str());
   return write(s);
 }