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.

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)
-    : producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false }
+  Pipeline(IProducer<T>& producer, IConsumer<T>& consumer, std::string name, INotifier& notifier)
+    : 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);
@@ -52,4 +52,4 @@ public:
   virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
   virtual bool setDigitalOut(uint8_t pin, bool value);
   virtual bool setAnalogOut(uint8_t pin, double value);
-};
+};