Parse unknown states to default RobotStates

include/ur_rtde_driver/comm/bin_parser.h

@@ -172,6 +172,14 @@ public:
     parse(val.rotation);
   }
 
+  void rawData(std::unique_ptr<uint8_t>& buffer, size_t& buffer_length)
+  {
+    buffer_length = buf_end_ - buf_pos_;
+    buffer.reset(new uint8_t[buffer_length]);
+    memcpy(buffer.get(), buf_pos_, buffer_length);
+    consume();
+  }
+
   void parseRemainder(std::string& val)
   {
     parse(val, size_t(buf_end_ - buf_pos_));

include/ur_rtde_driver/primary/primary_package.h

@@ -46,14 +46,17 @@ public:
   /*!
    * \brief Creates a new PrimaryPackage object.
    */
-  PrimaryPackage() = default;
+  PrimaryPackage() : buffer_length_(0)
+  {
+  }
   virtual ~PrimaryPackage() = default;
 
   virtual bool parseWith(comm::BinParser& bp);
   virtual std::string toString() const;
 
-private:
-  std::string buffer_;
+protected:
+  std::unique_ptr<uint8_t> buffer_;
+  size_t buffer_length_;
 };
 
 }  // namespace primary_interface

include/ur_rtde_driver/primary/primary_parser.h

@@ -144,9 +144,9 @@ private:
       case robot_state_type::MASTERBOARD_DATA:
         return new MBD;*/
       case RobotStateType::KINEMATICS_INFO:
-        return new KinematicsInfo;
+        return new KinematicsInfo(type);
       default:
-        return nullptr;
+        return new RobotState(type);
     }
   }
 

include/ur_rtde_driver/primary/robot_state.h

@@ -53,25 +53,31 @@ enum class RobotStateType : uint8_t
 };
 
 /*!
- * \brief Abstract class for a RobotState msg. This will never be instanciated, but the underlying
- * data packages will be used directly.
+ * \brief Base class for a RobotState data packages will be used directly.
  */
 class RobotState : public PrimaryPackage
 {
 public:
-  RobotState() = default;
+  RobotState() = delete;
+  RobotState(const RobotStateType type) : state_type_(type)
+  {
+  }
   virtual ~RobotState() = default;
 
   virtual bool parseWith(comm::BinParser& bp)
   {
-    return true;
+    return PrimaryPackage::parseWith(bp);
   }
   virtual std::string toString() const
   {
-    return std::string();
+    std::stringstream ss;
+    ss << "Type: " << static_cast<int>(state_type_) << std::endl;
+    ss << PrimaryPackage::toString();
+    return ss.str();
   }
 
 private:
+  RobotStateType state_type_;
 };
 
 }  // namespace primary_interface

src/primary/primary_package.cpp

@@ -33,13 +33,21 @@ namespace primary_interface
 {
 bool PrimaryPackage::parseWith(comm::BinParser& bp)
 {
-  bp.parseRemainder(buffer_);
+  bp.rawData(buffer_, buffer_length_);
   return true;
 }
 
 std::string PrimaryPackage::toString() const
 {
-  return buffer_;
+  std::stringstream ss;
+  ss << "Raw byte stream: ";
+  for (size_t i = 0; i < buffer_length_; ++i)
+  {
+    uint8_t* buf = buffer_.get();
+    ss << std::hex << static_cast<int>(buf[i]) << " ";
+  }
+  ss << std::endl;
+  return ss.str();
 }
 
 }  // namespace primary_interface