[primary interface] Fix parsing of primary messages (UniversalRobots#426)

- Message source is a signed integer
- Added missing MessageTypes
- Add raw data to RobotMessage if no specialization is known
- Correctly pass MessageType to RobotMessage
- Print string version of MessageType

Author: urfeex

include/ur_client_library/primary/primary_parser.h

@@ -185,7 +185,7 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
       case RobotMessagePackageType::ROBOT_MESSAGE_KEY:
         return new KeyMessage(timestamp, source);
       default:
-        return new RobotMessage(timestamp, source);
+        return new RobotMessage(timestamp, source, type);
     }
   }
 };

include/ur_client_library/primary/robot_message.h

@@ -106,7 +106,7 @@ class RobotMessage : public PrimaryPackage
   virtual std::string toString() const;
 
   uint64_t timestamp_;
-  uint8_t source_;
+  int8_t source_;
   RobotMessagePackageType message_type_;
 };
 

include/ur_client_library/primary/robot_state.h

@@ -50,9 +50,46 @@ enum class RobotStateType : uint8_t
   CONFIGURATION_DATA = 6,
   FORCE_MODE_DATA = 7,
   ADDITIONAL_INFO = 8,
-  CALIBRATION_DATA = 9
+  CALIBRATION_DATA = 9,
+  SAFETY_DATA = 10,
+  TOOL_COMM_INFO = 11,
+  TOOL_MODE_INFO = 12
 };
 
+inline const char* robotStateString(const RobotStateType state_type)
+{
+  switch (state_type)
+  {
+    case RobotStateType::ROBOT_MODE_DATA:
+      return "ROBOT_MODE_DATA";
+    case RobotStateType::JOINT_DATA:
+      return "JOINT_DATA";
+    case RobotStateType::TOOL_DATA:
+      return "TOOL_DATA";
+    case RobotStateType::MASTERBOARD_DATA:
+      return "MASTERBOARD_DATA";
+    case RobotStateType::CARTESIAN_INFO:
+      return "CARTESIAN_INFO";
+    case RobotStateType::KINEMATICS_INFO:
+      return "KINEMATICS_INFO";
+    case RobotStateType::CONFIGURATION_DATA:
+      return "CONFIGURATION_DATA";
+    case RobotStateType::FORCE_MODE_DATA:
+      return "FORCE_MODE_DATA";
+    case RobotStateType::ADDITIONAL_INFO:
+      return "ADDITIONAL_INFO";
+    case RobotStateType::CALIBRATION_DATA:
+      return "CALIBRATION_DATA";
+    case RobotStateType::SAFETY_DATA:
+      return "SAFETY_DATA";
+    case RobotStateType::TOOL_COMM_INFO:
+      return "TOOL_COMM_INFO";
+    case RobotStateType::TOOL_MODE_INFO:
+      return "TOOL_MODE_INFO";
+  }
+  return "";
+}
+
 /*!
  * \brief Base class for a RobotState data packages will be used directly.
  */

src/primary/robot_message.cpp

@@ -34,6 +34,7 @@ namespace primary_interface
 {
 bool RobotMessage::parseWith(comm::BinParser& bp)
 {
+  bp.rawData(buffer_, buffer_length_);
   return true;
 }
 
@@ -48,6 +49,7 @@ std::string RobotMessage::toString() const
   ss << "timestamp: " << timestamp_ << std::endl;
   ss << "source: " << static_cast<int>(source_) << std::endl;
   ss << "message_type: " << static_cast<int>(message_type_) << std::endl;
+  ss << PrimaryPackage::toString();
 
   return ss.str();
 }

src/primary/robot_state.cpp

@@ -47,7 +47,8 @@ bool RobotState::consumeWith(AbstractPrimaryConsumer& consumer)
 std::string RobotState::toString() const
 {
   std::stringstream ss;
-  ss << "Type: " << static_cast<int>(state_type_) << std::endl;
+  ss << "RobotState package of type: " << robotStateString(state_type_) << "( " << static_cast<int>(state_type_) << ")"
+     << std::endl;
   ss << PrimaryPackage::toString();
   return ss.str();
 }