Added and implemented primary messages

src/primary/robot_state/kinematics_info.cpp

@@ -0,0 +1,68 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-08
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+bool KinematicsInfo::parseWith(comm::BinParser& bp)
+{
+  bp.parse(checksum_);
+  bp.parse(dh_theta_);
+  bp.parse(dh_a_);
+  bp.parse(dh_d_);
+  bp.parse(dh_alpha_);
+  bp.parse(calibration_status_);
+
+  return true;
+}
+
+std::string KinematicsInfo::toString() const
+{
+  std::stringstream os;
+  os << "dh_theta: [";
+  for (size_t i = 0; i < dh_theta_.size(); ++i)
+  {
+    os << dh_theta_[i] << " ";
+  }
+  os << "]" << std::endl;
+
+  os << "dh_a: [";
+  for (size_t i = 0; i < dh_a_.size(); ++i)
+  {
+    os << dh_a_[i] << " ";
+  }
+  os << "]" << std::endl;
+
+  os << "dh_d: [";
+  for (size_t i = 0; i < dh_d_.size(); ++i)
+  {
+    os << dh_d_[i] << " ";
+  }
+  os << "]" << std::endl;
+
+  os << "dh_alpha: [";
+  for (size_t i = 0; i < dh_alpha_.size(); ++i)
+  {
+    os << dh_alpha_[i] << " ";
+  }
+  os << "]" << std::endl;
+
+  return os.str();
+}
+}  // namespace primary_interface
+}  // namespace ur_driver