From 4dc4cc0fb04f2bdb0c8a80389de261bf10e965e3 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 10 Apr 2019 08:30:50 +0200
Subject: [PATCH] Changed KinematicsInfo to match reality

In the docs, the message has a length of 222 bytes,
while the robots send 225 bytes of data.

As parsing goes correct, they have to be at the end of the message,
we assume that the calibration_data field in fact has the type of a 4-byte
integer.
---
 .../ur_rtde_driver/primary/robot_state/kinematics_info.h | 9 ++++++---
 src/primary/robot_state/kinematics_info.cpp              | 8 ++++++++
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/include/ur_rtde_driver/primary/robot_state/kinematics_info.h b/include/ur_rtde_driver/primary/robot_state/kinematics_info.h
index 660fb8d..5c8a450 100644
--- a/include/ur_rtde_driver/primary/robot_state/kinematics_info.h
+++ b/include/ur_rtde_driver/primary/robot_state/kinematics_info.h
@@ -29,18 +29,21 @@ namespace primary_interface
 class KinematicsInfo : public RobotState
 {
 public:
-  KinematicsInfo() = default;
+  KinematicsInfo() = delete;
+  KinematicsInfo(const RobotStateType type) : RobotState(type)
+  {
+  }
   virtual ~KinematicsInfo() = default;
 
   virtual bool parseWith(comm::BinParser& bp);
   virtual std::string toString() const;
 
-  vector6d_t checksum_;
+  vector6uint32_t checksum_;
   vector6d_t dh_theta_;
   vector6d_t dh_a_;
   vector6d_t dh_d_;
   vector6d_t dh_alpha_;
-  uint8_t calibration_status_;
+  uint32_t calibration_status_;  // According to the docs this should be uint8_t, but then I have 3 bytes left.
 };
 
 // TODO: Handle pre-3.6 as they don't have kinematics info
diff --git a/src/primary/robot_state/kinematics_info.cpp b/src/primary/robot_state/kinematics_info.cpp
index 77808ef..3a22bcb 100644
--- a/src/primary/robot_state/kinematics_info.cpp
+++ b/src/primary/robot_state/kinematics_info.cpp
@@ -34,6 +34,12 @@ bool KinematicsInfo::parseWith(comm::BinParser& bp)
 std::string KinematicsInfo::toString() const
 {
   std::stringstream os;
+  os << "checksum: [";
+  for (size_t i = 0; i < checksum_.size(); ++i)
+  {
+    os << checksum_[i] << " ";
+  }
+  os << "]" << std::endl;
   os << "dh_theta: [";
   for (size_t i = 0; i < dh_theta_.size(); ++i)
   {
@@ -62,6 +68,8 @@ std::string KinematicsInfo::toString() const
   }
   os << "]" << std::endl;
 
+  os << "calibration_status: " << calibration_status_ << std::endl;
+
   return os.str();
 }
 }  // namespace primary_interface