Tested producer part with real data

CMakeLists.txt

@@ -68,7 +68,6 @@ include_directories(
 
 add_library(ur_rtde_driver
     src/comm/tcp_socket.cpp
-    src/comm/shell_consumer.cpp
     #src/ros/service_stopper.cpp
     #src/ur/commander.cpp
     #src/ur/master_board.cpp

include/ur_rtde_driver/comm/package.h

@@ -46,8 +46,8 @@ public:
    * \brief Creates a new URPackage object.
    */
 
-  URPackage();
+  URPackage() = default;
-  virtual ~URPackage();
+  virtual ~URPackage() = default;
 
   using _header_type = HeaderT;
 

include/ur_rtde_driver/comm/parser.h

@@ -35,6 +35,9 @@ class Parser
 
 {
 public:
+  Parser() = default;
+  virtual ~Parser() = default;
+
   /*!
    * \brief declares the parse function.
    *

include/ur_rtde_driver/comm/pipeline.h

@@ -138,10 +138,44 @@ public:
 template <typename HeaderT>
 class Pipeline
 {
+public:
+  using _package_type = URPackage<HeaderT>;
+  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 }
+  {
+  }
+
+  void run()
+  {
+    if (running_)
+      return;
+
+    running_ = true;
+    pThread_ = std::thread(&Pipeline::run_producer, this);
+    cThread_ = std::thread(&Pipeline::run_consumer, this);
+    notifier_.started(name_);
+  }
+
+  void stop()
+  {
+    if (!running_)
+      return;
+
+    LOG_INFO("Stopping pipeline! <%s>", name_.c_str());
+
+    consumer_.stopConsumer();
+    producer_.stopProducer();
+
+    running_ = false;
+
+    pThread_.join();
+    cThread_.join();
+    notifier_.stopped(name_);
+  }
+
 private:
   typedef std::chrono::high_resolution_clock Clock;
   typedef Clock::time_point Time;
-  using _package_type = URPackage<HeaderT>;
   IProducer<HeaderT>& producer_;
   IConsumer<_package_type>& consumer_;
   std::string name_;
@@ -203,40 +237,6 @@ private:
     running_ = false;
     notifier_.stopped(name_);
   }
-
-public:
-  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 }
-  {
-  }
-
-  void run()
-  {
-    if (running_)
-      return;
-
-    running_ = true;
-    pThread_ = std::thread(&Pipeline::run_producer, this);
-    cThread_ = std::thread(&Pipeline::run_consumer, this);
-    notifier_.started(name_);
-  }
-
-  void stop()
-  {
-    if (!running_)
-      return;
-
-    LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
-
-    consumer_.stopConsumer();
-    producer_.stopProducer();
-
-    running_ = false;
-
-    pThread_.join();
-    cThread_.join();
-    notifier_.stopped(name_);
-  }
 };
 }  // namespace comm
 }  // namespace ur_driver

include/ur_rtde_driver/comm/producer.h

@@ -34,11 +34,11 @@ class URProducer : public IProducer<HeaderT>
 {
 private:
   URStream<HeaderT>& stream_;
-  URParser<HeaderT>& parser_;
+  Parser<HeaderT>& parser_;
   std::chrono::seconds timeout_;
 
 public:
-  URProducer(URStream<HeaderT>& stream, URParser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
+  URProducer(URStream<HeaderT>& stream, Parser<HeaderT>& parser) : stream_(stream), parser_(parser), timeout_(1)
   {
   }
 

include/ur_rtde_driver/comm/shell_consumer.h

@@ -15,6 +15,7 @@
 #ifndef UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED
 #define UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED
 
+#include "ur_rtde_driver/log.h"
 #include "ur_rtde_driver/comm/pipeline.h"
 #include "ur_rtde_driver/comm/package.h"
 
@@ -42,7 +43,11 @@ public:
   {
   }
 
-  virtual bool consume(std::shared_ptr<URPackage<HeaderT>> product);
+  virtual bool consume(std::shared_ptr<URPackage<HeaderT>> product)
+  {
+    LOG_INFO("%s", product->toString().c_str());
+    return true;
+  }
 
 private:
   /* data */

include/ur_rtde_driver/comm/stream.h

@@ -130,7 +130,7 @@ bool URStream<HeaderT>::read(uint8_t* buf, const size_t buf_len, size_t& total)
 
   bool initial = true;
   uint8_t* buf_pos = buf;
-  size_t remainder = sizeof(HeaderT::_package_size_type);
+  size_t remainder = sizeof(typename HeaderT::_package_size_type);
   size_t read = 0;
 
   while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
@@ -139,7 +139,7 @@ bool URStream<HeaderT>::read(uint8_t* buf, const size_t buf_len, size_t& total)
     if (initial)
     {
       remainder = HeaderT::getPackageLength(buf);
-      if (remainder >= (buf_len - sizeof(HeaderT::_package_size_type)))
+      if (remainder >= (buf_len - sizeof(typename HeaderT::_package_size_type)))
       {
         LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
         return false;

include/ur_rtde_driver/primary/primary_parser.h

@@ -24,9 +24,9 @@
 #include "ur_rtde_driver/primary/package_header.h"
 #include "ur_rtde_driver/primary/robot_state.h"
 #include "ur_rtde_driver/primary/robot_message.h"
-#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h"
+//#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h"
 #include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
-#include "ur_rtde_driver/primary/robot_state/master_board.h"
+//#include "ur_rtde_driver/primary/robot_state/master_board.h"
 #include "ur_rtde_driver/primary/robot_message/version_message.h"
 
 namespace ur_driver
@@ -34,54 +34,22 @@ namespace ur_driver
 namespace primary_interface
 {
 using namespace comm;
-class PrimaryParser : comm::Parser<PackageHeader>
+class PrimaryParser : public comm::Parser<PackageHeader>
 {
-private:
-  RobotState* state_from_type(robot_state_type type)
-  {
-    switch (type)
-    {
-      /*case robot_state_type::ROBOT_MODE_DATA:
-        // SharedRobotModeData* rmd = new SharedRobotModeData();
-
-        //return new rmd;
-      case robot_state_type::MASTERBOARD_DATA:
-        return new MBD;*/
-      case robot_state_type::KINEMATICS_INFO:
-        return new KinematicsInfo;
-      default:
-        return nullptr;
-    }
-  }
-
-  RobotMessage* message_from_type(message_type type, uint64_t timestamp, uint8_t source)
-  {
-    switch (type)
-    {
-      /*case robot_state_type::ROBOT_MODE_DATA:
-        // SharedRobotModeData* rmd = new SharedRobotModeData();
-
-        //return new rmd;
-      case robot_state_type::MASTERBOARD_DATA:
-        return new MBD;*/
-      case message_type::ROBOT_MESSAGE_VERSION:
-        return new VersionMessage(timestamp, source);
-      default:
-        return nullptr;
-    }
-  }
-
 public:
-  bool parse(BinParser& bp, std::vector<std::unique_ptr<PrimaryPackage>>& results)
+  PrimaryParser() = default;
+  virtual ~PrimaryParser() = default;
+
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<URPackage<PackageHeader>>>& results)
   {
     int32_t packet_size;
-    robot_message_type type;
+    RobotPackageType type;
     bp.parse(packet_size);
     bp.parse(type);
 
     switch (type)
     {
-      case robot_message_type::ROBOT_STATE:
+      case RobotPackageType::ROBOT_STATE:
       {
         while (!bp.empty())
         {
@@ -100,7 +68,7 @@ public:
           // deconstruction of a sub parser will increment the position of the parent parser
           BinParser sbp(bp, sub_size);
           sbp.consume(sizeof(sub_size));
-          robot_state_type type;
+          RobotStateType type;
           sbp.parse(type);
 
           std::unique_ptr<PrimaryPackage> packet(state_from_type(type));
@@ -132,18 +100,16 @@ public:
         break;
       }
 
-      case robot_message_type::ROBOT_MESSAGE:
+      case RobotPackageType::ROBOT_MESSAGE:
       {
         uint64_t timestamp;
         uint8_t source;
-        message_type message_type;
+        RobotMessagePackageType message_type;
 
         bp.parse(timestamp);
         bp.parse(source);
         bp.parse(message_type);
 
-        bool parsed = false;
-
         std::unique_ptr<PrimaryPackage> packet(message_from_type(message_type, timestamp, source));
         if (!packet->parseWith(bp))
         {
@@ -152,19 +118,54 @@ public:
         }
 
         results.push_back(std::move(packet));
-        return parsed;
+        return true;
         break;
       }
 
       default:
       {
-        LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
+        LOG_WARN("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
         bp.consume();
         return true;
       }
     }
     return true;
   }
+
+private:
+  RobotState* state_from_type(RobotStateType type)
+  {
+    switch (type)
+    {
+      /*case robot_state_type::ROBOT_MODE_DATA:
+        // SharedRobotModeData* rmd = new SharedRobotModeData();
+
+        //return new rmd;
+      case robot_state_type::MASTERBOARD_DATA:
+        return new MBD;*/
+      case RobotStateType::KINEMATICS_INFO:
+        return new KinematicsInfo;
+      default:
+        return nullptr;
+    }
+  }
+
+  RobotMessage* message_from_type(RobotMessagePackageType type, uint64_t timestamp, uint8_t source)
+  {
+    switch (type)
+    {
+      /*case robot_state_type::ROBOT_MODE_DATA:
+        // SharedRobotModeData* rmd = new SharedRobotModeData();
+
+        //return new rmd;
+      case robot_state_type::MASTERBOARD_DATA:
+        return new MBD;*/
+      case RobotMessagePackageType::ROBOT_MESSAGE_VERSION:
+        return new VersionMessage(timestamp, source);
+      default:
+        return new RobotMessage(timestamp, source);
+    }
+  }
 };
 
 }  // namespace primary_interface

include/ur_rtde_driver/primary/robot_state.h

@@ -38,6 +38,20 @@ namespace ur_driver
 {
 namespace primary_interface
 {
+enum class RobotStateType : uint8_t
+{
+  ROBOT_MODE_DATA = 0,
+  JOINT_DATA = 1,
+  TOOL_DATA = 2,
+  MASTERBOARD_DATA = 3,
+  CARTESIAN_INFO = 4,
+  KINEMATICS_INFO = 5,
+  CONFIGURATION_DATA = 6,
+  FORCE_MODE_DATA = 7,
+  ADDITIONAL_INFO = 8,
+  CALIBRATION_DATA = 9
+};
+
 /*!
  * \brief Abstract class for a RobotState msg. This will never be instanciated, but the underlying
  * data packages will be used directly.

include/ur_rtde_driver/primary/robot_state/robot_mode_data.h

@@ -38,7 +38,7 @@ namespace primary_interface
 class SharedRobotModeData
 {
 public:
-  virtual bool parseWith(BinParser& bp);
+  virtual bool parseWith(comm::BinParser& bp);
 
   uint64_t timestamp;
   bool physical_robot_connected;
@@ -76,7 +76,7 @@ enum class robot_control_mode_V3_X : uint8_t
 class RobotModeData_V3_0__1 : public SharedRobotModeData, public RobotState
 {
 public:
-  virtual bool parseWith(BinParser& bp);
+  virtual bool parseWith(comm::BinParser& bp);
 
   robot_mode_V3_X robot_mode;
   robot_control_mode_V3_X control_mode;
@@ -93,8 +93,8 @@ public:
 class RobotModeData_V3_2 : public RobotModeData_V3_0__1
 {
 public:
-  virtual bool parseWith(BinParser& bp);
+  virtual bool parseWith(comm::BinParser& bp);
-  virtual bool consumeWith(URStatePacketConsumer& consumer);
+  // virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   double target_speed_fraction_limit;
 
@@ -106,8 +106,8 @@ public:
 class RobotModeData_V3_5 : public RobotModeData_V3_2
 {
 public:
-  virtual bool parseWith(BinParser& bp);
+  virtual bool parseWith(comm::BinParser& bp);
-  virtual bool consumeWith(URStatePacketConsumer& consumer);
+  // virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   unsigned char unknown_internal_use;
 

src/comm/shell_consumer.cpp

@@ -1,29 +0,0 @@
-// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
-
-// -- BEGIN LICENSE BLOCK ----------------------------------------------
-// -- END LICENSE BLOCK ------------------------------------------------
-
-//----------------------------------------------------------------------
-/*!\file
- *
- * \author  Felix Mauch mauch@fzi.de
- * \date    2019-04-09
- *
- */
-//----------------------------------------------------------------------
-
-#include "ur_rtde_driver/comm/shell_consumer.h"
-
-namespace ur_driver
-{
-namespace comm
-{
-template <typename HeaderT>
-bool ShellConsumer<HeaderT>::consume(std::shared_ptr<URPackage<HeaderT>> pkg)
-{
-  LOG_INFO("%s", pkg->toString());
-  return true;
-}
-}  // namespace comm
-
-}  // namespace ur_driver

src/primary/robot_message.cpp

@@ -32,10 +32,6 @@ namespace primary_interface
 {
 bool RobotMessage::parseWith(comm::BinParser& bp)
 {
-  bp.parse(timestamp_);
-  bp.parse(source_);
-  bp.parse(message_type_);
-
   return true;
 }