Merge pull request #166 from UniversalRobots/packaget

Use the package type and not the header type as template parameter for communication

Author: Felix Exner

ur_calibration/include/ur_calibration/calibration_consumer.h

@@ -35,15 +35,13 @@
 
 namespace ur_calibration
 {
-class CalibrationConsumer
-  : public ur_driver::comm::IConsumer<ur_driver::comm::URPackage<ur_driver::primary_interface::PackageHeader>>
+class CalibrationConsumer : public ur_driver::comm::IConsumer<ur_driver::primary_interface::PrimaryPackage>
 {
 public:
   CalibrationConsumer();
   virtual ~CalibrationConsumer() = default;
 
-  virtual bool
-  consume(std::shared_ptr<ur_driver::comm::URPackage<ur_driver::primary_interface::PackageHeader>> product);
+  virtual bool consume(std::shared_ptr<ur_driver::primary_interface::PrimaryPackage> product);
 
   bool isCalibrated() const
   {

ur_calibration/src/calibration_consumer.cpp

@@ -33,8 +33,7 @@ CalibrationConsumer::CalibrationConsumer() : calibrated_(false)
 {
 }
 
-bool CalibrationConsumer::consume(
-    std::shared_ptr<ur_driver::comm::URPackage<ur_driver::primary_interface::PackageHeader>> product)
+bool CalibrationConsumer::consume(std::shared_ptr<ur_driver::primary_interface::PrimaryPackage> product)
 {
   auto kin_info = std::dynamic_pointer_cast<ur_driver::primary_interface::KinematicsInfo>(product);
   if (kin_info != nullptr)

ur_calibration/src/calibration_correction.cpp

@@ -80,14 +80,14 @@ class CalibrationCorrection
 
   void run()
   {
-    comm::URStream<PackageHeader> stream(robot_ip_, UR_PRIMARY_PORT);
+    comm::URStream<PrimaryPackage> stream(robot_ip_, UR_PRIMARY_PORT);
     primary_interface::PrimaryParser parser;
-    comm::URProducer<PackageHeader> prod(stream, parser);
+    comm::URProducer<PrimaryPackage> prod(stream, parser);
     CalibrationConsumer consumer;
 
     comm::INotifier notifier;
 
-    comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
+    comm::Pipeline<PrimaryPackage> pipeline(prod, &consumer, "Pipeline", notifier);
     pipeline.run();
     while (!consumer.isCalibrated())
     {

ur_robot_driver/CMakeLists.txt

@@ -89,6 +89,7 @@ add_library(ur_robot_driver
     #src/ur/robot_mode.cpp
     src/primary/primary_package.cpp
     src/primary/robot_message.cpp
+    src/primary/robot_state.cpp
     src/primary/robot_message/version_message.cpp
     src/primary/robot_state/kinematics_info.cpp
     src/rtde/control_package_pause.cpp

ur_robot_driver/include/ur_robot_driver/comm/package.h

@@ -66,6 +66,8 @@ class URPackage
    */
   virtual std::string toString() const = 0;
 
+  using HeaderType = HeaderT;
+
 private:
   HeaderT header_;
 };

ur_robot_driver/include/ur_robot_driver/comm/parser.h

@@ -32,7 +32,7 @@ namespace comm
  * iclude classes which inherit from it (rtdeParser and primaryParser).
  * The parser functionality also embodies a factory function taking in an uint8.
  */
-template <typename HeaderT>
+template <typename T>
 class Parser
 
 {
@@ -46,11 +46,10 @@ class Parser
    * \param bp Instance of class binaryParser
    * \param results A unique pointer
    */
-  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<URPackage<HeaderT>>>& results) = 0;
-  using _header_type = HeaderT;
+  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
 
 private:
-  HeaderT header_;
+  typename T::HeaderType header_;
   // URProducer producer_;
 };
 

ur_robot_driver/include/ur_robot_driver/comm/pipeline.h

@@ -163,9 +163,9 @@ class MultiConsumer : public IConsumer<T>
 /*!
  * \brief Parent class for arbitrary producers of packages.
  *
- * @tparam HeaderT Header type of the produced packages
+ * @tparam T Type of the produced products
  */
-template <typename HeaderT>
+template <typename T>
 class IProducer
 {
 public:
@@ -200,7 +200,7 @@ class IProducer
    *
    * \returns Success of the package production.
    */
-  virtual bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products) = 0;
+  virtual bool tryGet(std::vector<std::unique_ptr<T>>& products) = 0;
 };
 
 /*!
@@ -228,15 +228,14 @@ class INotifier
  * the producer is called and returned packages are saved in a queue. This queue is then either also
  * cyclically utilized by the registered consumer or can be externally used.
  *
- * @tparam HeaderT Header type of the managed packages
+ * @tparam T Type of the managed packages
  */
-template <typename HeaderT>
+template <typename T>
 class Pipeline
 {
 public:
   typedef std::chrono::high_resolution_clock Clock;
   typedef Clock::time_point Time;
-  using _package_type = URPackage<HeaderT>;
   /*!
    * \brief Creates a new Pipeline object, registering producer, consumer and notifier.
    * Additionally, an empty queue is initialized.
@@ -246,8 +245,8 @@ class Pipeline
    * \param name The pipeline's name
    * \param notifier The notifier to use
    */
-  Pipeline(IProducer<HeaderT>& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier)
-    : producer_(producer), consumer_(&consumer), name_(name), notifier_(notifier), 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 }
   {
   }
   /*!
@@ -258,7 +257,7 @@ class Pipeline
    * \param name The pipeline's name
    * \param notifier The notifier to use
    */
-  Pipeline(IProducer<HeaderT>& producer, std::string name, INotifier& notifier)
+  Pipeline(IProducer<T>& producer, std::string name, INotifier& notifier)
     : producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
   {
   }
@@ -327,17 +326,17 @@ class Pipeline
    *
    * \returns
    */
-  bool getLatestProduct(std::unique_ptr<URPackage<HeaderT>>& product, std::chrono::milliseconds timeout)
+  bool getLatestProduct(std::unique_ptr<T>& product, std::chrono::milliseconds timeout)
   {
     return queue_.waitDequeTimed(product, timeout);
   }
 
 private:
-  IProducer<HeaderT>& producer_;
-  IConsumer<_package_type>* consumer_;
+  IProducer<T>& producer_;
+  IConsumer<T>* consumer_;
   std::string name_;
   INotifier& notifier_;
-  moodycamel::BlockingReaderWriterQueue<std::unique_ptr<_package_type>> queue_;
+  moodycamel::BlockingReaderWriterQueue<std::unique_ptr<T>> queue_;
   std::atomic<bool> running_;
   std::thread pThread_, cThread_;
 
@@ -396,7 +395,7 @@ class Pipeline
     {
       LOG_WARN("No realtime capabilities found. Consider using a realtime system for better performance");
     }
-    std::vector<std::unique_ptr<_package_type>> products;
+    std::vector<std::unique_ptr<T>> products;
     while (running_)
     {
       if (!producer_.tryGet(products))
@@ -422,7 +421,7 @@ class Pipeline
 
   void runConsumer()
   {
-    std::unique_ptr<_package_type> product;
+    std::unique_ptr<T> product;
     while (running_)
     {
       // timeout was chosen because we should receive messages

ur_robot_driver/include/ur_robot_driver/comm/producer.h

@@ -36,12 +36,12 @@ namespace comm
  *
  * @tparam HeaderT Header type of packages to produce.
  */
-template <typename HeaderT>
-class URProducer : public IProducer<HeaderT>
+template <typename T>
+class URProducer : public IProducer<T>
 {
 private:
-  URStream<HeaderT>& stream_;
-  Parser<HeaderT>& parser_;
+  URStream<T>& stream_;
+  Parser<T>& parser_;
   std::chrono::seconds timeout_;
 
   bool running_;
@@ -53,8 +53,7 @@ class URProducer : public IProducer<HeaderT>
    * \param stream The stream to read from
    * \param parser The parser to use to interpret received byte information
    */
-  URProducer(URStream<HeaderT>& stream, Parser<HeaderT>& parser)
-    : stream_(stream), parser_(parser), timeout_(1), running_(false)
+  URProducer(URStream<T>& stream, Parser<T>& parser) : stream_(stream), parser_(parser), timeout_(1), running_(false)
   {
   }
 
@@ -99,7 +98,7 @@ class URProducer : public IProducer<HeaderT>
    *
    * \returns Success of reading and parsing the package
    */
-  bool tryGet(std::vector<std::unique_ptr<URPackage<HeaderT>>>& products) override
+  bool tryGet(std::vector<std::unique_ptr<T>>& products) override
   {
     // TODO This function has become really ugly! That should be refactored!
 

ur_robot_driver/include/ur_robot_driver/comm/shell_consumer.h

@@ -42,8 +42,8 @@ namespace comm
  *
  * @tparam HeaderT Header type of the packages to consume
  */
-template <typename HeaderT>
-class ShellConsumer : public IConsumer<URPackage<HeaderT>>
+template <typename T>
+class ShellConsumer : public IConsumer<T>
 {
 public:
   ShellConsumer() = default;
@@ -56,7 +56,7 @@ class ShellConsumer : public IConsumer<URPackage<HeaderT>>
    *
    * \returns True if the output was successful
    */
-  virtual bool consume(std::shared_ptr<URPackage<HeaderT>> product)
+  virtual bool consume(std::shared_ptr<T> product)
   {
     LOG_INFO("%s", product->toString().c_str());
     return true;

ur_robot_driver/include/ur_robot_driver/comm/stream.h

@@ -38,7 +38,7 @@ namespace comm
  * peek at the field defining the package length. This is why it is templated with the package
  * header type.
  */
-template <typename HeaderT>
+template <typename T>
 class URStream : public TCPSocket
 {
 public:
@@ -116,30 +116,30 @@ class URStream : public TCPSocket
   std::mutex write_mutex_, read_mutex_;
 };
 
-template <typename HeaderT>
-bool URStream<HeaderT>::write(const uint8_t* buf, const size_t buf_len, size_t& written)
+template <typename T>
+bool URStream<T>::write(const uint8_t* buf, const size_t buf_len, size_t& written)
 {
   std::lock_guard<std::mutex> lock(write_mutex_);
   return TCPSocket::write(buf, buf_len, written);
 }
 
-template <typename HeaderT>
-bool URStream<HeaderT>::read(uint8_t* buf, const size_t buf_len, size_t& total)
+template <typename T>
+bool URStream<T>::read(uint8_t* buf, const size_t buf_len, size_t& total)
 {
   std::lock_guard<std::mutex> lock(read_mutex_);
 
   bool initial = true;
   uint8_t* buf_pos = buf;
-  size_t remainder = sizeof(typename HeaderT::_package_size_type);
+  size_t remainder = sizeof(typename T::HeaderType::_package_size_type);
   size_t read = 0;
 
   while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
   {
     TCPSocket::setOptions(getSocketFD());
     if (initial)
     {
-      remainder = HeaderT::getPackageLength(buf);
-      if (remainder >= (buf_len - sizeof(typename HeaderT::_package_size_type)))
+      remainder = T::HeaderType::getPackageLength(buf);
+      if (remainder >= (buf_len - sizeof(typename T::HeaderType::_package_size_type)))
       {
         LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
         return false;