Merge current development into release

ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h

@@ -31,6 +31,7 @@
 #include <memory>
 #include "ur_rtde_driver/log.h"
 #include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/exceptions.h"
 
 namespace ur_driver
 {
@@ -93,7 +94,10 @@ public:
   template <typename T>
   T peek()
   {
-    assert(buf_pos_ + sizeof(T) <= buf_end_);
+    if (buf_pos_ + sizeof(T) > buf_end_)
+      throw UrException("Could not parse received package. This can occur if the driver is started while the robot is "
+                        "booting - please restart the driver once the robot has finished booting. "
+                        "If the problem persists after the robot has booted, please contact the package maintainer.");
     T val;
     std::memcpy(&val, buf_pos_, sizeof(T));
     return decode(val);

ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_parser.h

@@ -42,7 +42,7 @@ class RTDEParser : public comm::Parser<PackageHeader>
 {
 public:
   RTDEParser() = delete;
-  RTDEParser(const std::vector<std::string>& recipe) : recipe_(recipe)
+  RTDEParser(const std::vector<std::string>& recipe) : recipe_(recipe), protocol_version_(1)
   {
   }
   virtual ~RTDEParser() = default;
@@ -98,6 +98,11 @@ public:
     return true;
   }
 
+  void setProtocolVersion(uint16_t protocol_version)
+  {
+    protocol_version_ = protocol_version;
+  }
+
 private:
   std::vector<std::string> recipe_;
   RTDEPackage* packageFromType(PackageType type)
@@ -105,7 +110,7 @@ private:
     switch (type)
     {
       case PackageType::RTDE_TEXT_MESSAGE:
-        return new TextMessage;
+        return new TextMessage(protocol_version_);
         break;
       case PackageType::RTDE_GET_URCONTROL_VERSION:
         return new GetUrcontrolVersion;
@@ -129,6 +134,7 @@ private:
         return new RTDEPackage(type);
     }
   }
+  uint16_t protocol_version_;
 };
 
 }  // namespace rtde_interface

ur_rtde_driver/include/ur_rtde_driver/rtde/text_message.h

@@ -37,7 +37,8 @@ namespace rtde_interface
 class TextMessage : public RTDEPackage
 {
 public:
-  TextMessage() : RTDEPackage(PackageType::RTDE_TEXT_MESSAGE)
+  TextMessage(uint16_t protocol_version)
+    : RTDEPackage(PackageType::RTDE_TEXT_MESSAGE), protocol_version_(protocol_version)
   {
   }
   virtual ~TextMessage() = default;
@@ -50,6 +51,10 @@ public:
   uint8_t source_length_;
   std::string source_;
   uint8_t warning_level_;
+
+  uint8_t message_type_;
+
+  uint16_t protocol_version_;
 };
 
 }  // namespace rtde_interface

ur_rtde_driver/src/rtde/rtde_client.cpp

@@ -74,6 +74,8 @@ bool RTDEClient::init()
     }
   }
 
+  parser_.setProtocolVersion(protocol_version);
+
   // determine maximum frequency from ur-control version
   size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer);
   stream_.write(buffer, size, written);

ur_rtde_driver/src/rtde/text_message.cpp

@@ -33,11 +33,19 @@ namespace rtde_interface
 {
 bool TextMessage::parseWith(comm::BinParser& bp)
 {
-  bp.parse(message_length_);
-  bp.parse(message_, message_length_);
-  bp.parse(source_length_);
-  bp.parse(source_, source_length_);
-  bp.parse(warning_level_);
+  if (protocol_version_ == 2)
+  {
+    bp.parse(message_length_);
+    bp.parse(message_, message_length_);
+    bp.parse(source_length_);
+    bp.parse(source_, source_length_);
+    bp.parse(warning_level_);
+  }
+  else if (protocol_version_ == 1)
+  {
+    bp.parse(message_type_);
+    bp.parseRemainder(message_);
+  }
 
   return true;
 }