Parse unknown states to default RobotStates

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