Tested producer part with real data

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 consumeWith(URStatePacketConsumer& consumer);
+  virtual bool parseWith(comm::BinParser& bp);
+  // 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 consumeWith(URStatePacketConsumer& consumer);
+  virtual bool parseWith(comm::BinParser& bp);
+  // virtual bool consumeWith(URStatePacketConsumer& consumer);
 
   unsigned char unknown_internal_use;