From bf857557447d106a750a65e5f23892744faac9ce Mon Sep 17 00:00:00 2001
From: "Simon Schmeisser (isys vision)" <simon.schmeisser@isys-vision.de>
Date: Mon, 8 Jan 2018 09:14:54 +0100
Subject: [PATCH] Add support for version 3.5

A new undocumented uchar was added to RobotMode
---
 include/ur_modern_driver/ur/factory.h      |  4 +++-
 include/ur_modern_driver/ur/robot_mode.h   | 15 ++++++++++++++-
 include/ur_modern_driver/ur/state_parser.h |  1 +
 src/ur/robot_mode.cpp                      | 18 +++++++++++++++++-
 4 files changed, 35 insertions(+), 3 deletions(-)

diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h
index ecf3bed..54c6cdf 100644
--- a/include/ur_modern_driver/ur/factory.h
+++ b/include/ur_modern_driver/ur/factory.h
@@ -94,8 +94,10 @@ public:
     {
       if (minor_version_ < 3)
         return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
-      else
+      else if (minor_version_ < 5)
         return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
+      else
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_5);
     }
   }
 
diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h
index 80aa145..c6a1cfc 100644
--- a/include/ur_modern_driver/ur/robot_mode.h
+++ b/include/ur_modern_driver/ur/robot_mode.h
@@ -102,4 +102,17 @@ public:
   static const size_t SIZE = RobotModeData_V3_0__1::SIZE + sizeof(double);
 
   static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
-};
\ No newline at end of file
+};
+
+class RobotModeData_V3_5 : public RobotModeData_V3_2
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  unsigned char unknown_internal_use;
+
+  static const size_t SIZE = RobotModeData_V3_2::SIZE + sizeof(unsigned char);
+
+  static_assert(RobotModeData_V3_5::SIZE == 42, "RobotModeData_V3_5 has missmatched size");
+};
diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h
index 146d93c..37da43c 100644
--- a/include/ur_modern_driver/ur/state_parser.h
+++ b/include/ur_modern_driver/ur/state_parser.h
@@ -96,3 +96,4 @@ public:
 typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
 typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
 typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
+typedef URStateParser<RobotModeData_V3_5, MasterBoardData_V3_2> URStateParser_V3_5;
diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp
index da3a65b..783648b 100644
--- a/src/ur/robot_mode.cpp
+++ b/src/ur/robot_mode.cpp
@@ -54,6 +54,18 @@ bool RobotModeData_V3_2::parseWith(BinParser& bp)
   return true;
 }
 
+bool RobotModeData_V3_5::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RobotModeData_V3_5>())
+    return false;
+
+  RobotModeData_V3_2::parseWith(bp);
+
+  bp.parse(unknown_internal_use);
+
+  return true;
+}
+
 bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
 {
   return consumer.consume(*this);
@@ -65,4 +77,8 @@ bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
 bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
 {
   return consumer.consume(*this);
-}
\ No newline at end of file
+}
+bool RobotModeData_V3_5::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}