Added and implemented primary messages

CMakeLists.txt

@@ -68,13 +68,15 @@ include_directories(
 
 add_library(ur_rtde_driver
     src/comm/tcp_socket.cpp
-    src/ros/service_stopper.cpp
-    src/ur/commander.cpp
-    src/ur/master_board.cpp
-    src/ur/messages.cpp
-    src/ur/robot_mode.cpp
-    src/ur/rt_state.cpp
-    src/ur/server.cpp
+    #src/ros/service_stopper.cpp
+    #src/ur/commander.cpp
+    #src/ur/master_board.cpp
+    #src/ur/messages.cpp
+    #src/ur/robot_mode.cpp
+    #src/ur/rt_state.cpp
+    #src/ur/server.cpp
+    src/primary/robot_message/version_message.cpp
+    src/primary/robot_state/kinematics_info.cpp
 )
 target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
 add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

include/ur_rtde_driver/comm/bin_parser.h

@@ -133,6 +133,38 @@ public:
     parse(val.z);
   }
 
+  void parse(vector3d_t& val)
+  {
+    for (size_t i = 0; i < val.size(); ++i)
+    {
+      parse(val[i]);
+    }
+  }
+
+  void parse(vector6d_t& val)
+  {
+    for (size_t i = 0; i < val.size(); ++i)
+    {
+      parse(val[i]);
+    }
+  }
+
+  void parse(vector6int32_t& val)
+  {
+    for (size_t i = 0; i < val.size(); ++i)
+    {
+      parse(val[i]);
+    }
+  }
+
+  void parse(vector6uint32_t& val)
+  {
+    for (size_t i = 0; i < val.size(); ++i)
+    {
+      parse(val[i]);
+    }
+  }
+
   // Explicit parsing order of fields to avoid issues with struct layout
   void parse(cartesian_coord_t& val)
   {
@@ -140,7 +172,7 @@ public:
     parse(val.rotation);
   }
 
-  void parse_remainder(std::string& val)
+  void parseRemainder(std::string& val)
   {
     parse(val, size_t(buf_end_ - buf_pos_));
   }

include/ur_rtde_driver/comm/package.h

@@ -44,6 +44,8 @@ public:
 
   virtual bool parseWith(BinParser& bp) = 0;
 
+  virtual std::string toString() const = 0;
+
 private:
   HeaderT header_;
 };

include/ur_rtde_driver/primary/robot_message.h

@@ -50,8 +50,17 @@ enum class message_type : uint8_t
 class RobotMessage : PrimaryPackage
 {
 public:
-  RobotMessage() = default;
+  RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source)
+  {
+  }
   virtual ~RobotMessage() = default;
+
+  virtual bool parseWith(comm::BinParser& bp) = 0;
+  virtual std::string toString() const = 0;
+
+  uint64_t timestamp_;
+  uint8_t source_;
+  uint8_t message_type_;
 };
 
 }  // namespace primary_interface

include/ur_rtde_driver/primary/robot_message/version_message.h

@@ -19,33 +19,40 @@
 //----------------------------------------------------------------------
 /*!\file
  *
- * \author  Lea Steffen steffen@fzi.de
- * \date    2019-04-01
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-08
  *
  */
 //----------------------------------------------------------------------
 
-#ifndef UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED
-#define UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED
+#ifndef UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED
+#define UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED
 
-#include "ur_rtde_driver/primary/primary_package.h"
-#include "ur_rtde_driver/primary/robot_state.h"
+#include "ur_rtde_driver/primary/robot_message.h"
 
 namespace ur_driver
 {
 namespace primary_interface
 {
-class KinematicsInfo : PrimaryPackage
+class VersionMessage : public RobotMessage
 {
-private:
-  RobotState robot_state_;
-
 public:
-  KinematicsInfo() = default;
-  virtual ~KinematicsInfo() = default;
-};
+  VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source)
+  {
+  }
 
+  virtual bool parseWith(comm::BinParser& bp);
+
+  virtual std::string toString() const;
+
+  std::string project_name_;
+  uint8_t major_version_;
+  uint8_t minor_version_;
+  int32_t svn_version_;
+  int32_t build_number_;  // TODO Exists in version 3.3 above only
+  std::string build_date_;
+};
 }  // namespace primary_interface
 }  // namespace ur_driver
 
-#endif /* UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED */
+#endif  // ifndef UR_RTDE_DRIVER_VERSION_MESSAGE_H_INCLUDED

include/ur_rtde_driver/primary/robot_state.h

@@ -38,91 +38,17 @@ namespace ur_driver
 {
 namespace primary_interface
 {
-enum class robot_state_type : uint8_t
+/*!
+ * \brief Abstract class for a RobotState msg. This will never be instanciated, but the underlying
+ * data packages will be used directly.
+ */
+class RobotState : public PrimaryPackage
 {
-  ROBOT_MODE_DATA = 0,
-  JOINT_DATA = 1,
-  TOOL_DATA = 2,
-  MASTERBOARD_DATA = 3,
-  CARTESIAN_INFO = 4,
-  KINEMATICS_INFO = 5,
-  CONFIGURATION_DATA = 6,
-  FORCE_MODE_DATA = 7,
-  ADDITIONAL_INFO = 8,
-  CALIBRATION_DATA = 9
-};
-
-struct version_message
-{
-  uint64_t timestamp;
-  int8_t source;
-  int8_t robot_message_type;
-  int8_t project_name_size;
-  char project_name[15];
-  uint8_t major_version;
-  uint8_t minor_version;
-  int svn_revision;
-  char build_date[25];
-};
-
-struct masterboard_data
-{
-  int digitalInputBits;
-  int digitalOutputBits;
-  char analogInputRange0;
-  char analogInputRange1;
-  double analogInput0;
-  double analogInput1;
-  char analogOutputDomain0;
-  char analogOutputDomain1;
-  double analogOutput0;
-  double analogOutput1;
-  float masterBoardTemperature;
-  float robotVoltage48V;
-  float robotCurrent;
-  float masterIOCurrent;
-  unsigned char safetyMode;
-  unsigned char masterOnOffState;
-  char euromap67InterfaceInstalled;
-  int euromapInputBits;
-  int euromapOutputBits;
-  float euromapVoltage;
-  float euromapCurrent;
-};
-
-struct robot_mode_data
-{
-  uint64_t timestamp;
-  bool isRobotConnected;
-  bool isRealRobotEnabled;
-  bool isPowerOnRobot;
-  bool isEmergencyStopped;
-  bool isProtectiveStopped;
-  bool isProgramRunning;
-  bool isProgramPaused;
-  unsigned char robotMode;
-  unsigned char controlMode;
-  double targetSpeedFraction;
-  double speedScaling;
-};
-
-class RobotState : PrimaryPackage
-{
-private:
-  robot_state_type robot_state_;
-  /*
-      version_message version_msg_;
-    masterboard_data mb_data_;
-    robot_mode_data robot_mode_;
-      std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
-    std::condition_variable* pMsg_cond_; //Signals that new vars are available
-    bool new_data_available_; //to avoid spurious wakes
-    unsigned char robot_mode_running_;
-  double ntohd(uint64_t nf); */
-
 public:
   RobotState() = default;
   virtual ~RobotState() = default;
+
+private:
 };
 
 }  // namespace primary_interface

include/ur_rtde_driver/primary/robot_state/kinematics_info.h

@@ -0,0 +1,50 @@
+// 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
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED
+#define UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED
+
+#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/primary/robot_state.h"
+namespace ur_driver
+{
+namespace primary_interface
+{
+/*!
+ * \brief This messages contains information about the robot's calibration. The DH parameters are
+ * a combination between the perfect model parameters and the correction deltas as noted in the
+ * configuration files on the robot controller.
+ */
+class KinematicsInfo : RobotState
+{
+public:
+  KinematicsInfo() = default;
+  virtual ~KinematicsInfo() = default;
+
+  virtual bool parseWith(comm::BinParser& bp);
+  virtual std::string toString() const;
+
+  vector6d_t checksum_;
+  vector6d_t dh_theta_;
+  vector6d_t dh_a_;
+  vector6d_t dh_d_;
+  vector6d_t dh_alpha_;
+  uint8_t calibration_status_;
+};
+
+// TODO: Handle pre-3.6 as they don't have kinematics info
+}  // namespace primary_interface
+}  // namespace ur_driver
+
+#endif  // ifndef UR_RTDE_DRIVER_KINEMATICS_INFO_H_INCLUDED

include/ur_rtde_driver/primary/robot_state/master_board.h

@@ -21,12 +21,12 @@
 #include <inttypes.h>
 #include <bitset>
 #include <cstddef>
-#include "ur_rtde_driver/bin_parser.h"
-#include "ur_rtde_driver/types.h"
-#include "ur_rtde_driver/ur/state.h"
+#include "ur_rtde_driver/primary/robot_state.h"
 
 namespace ur_driver
 {
+namespace primary_interface
+{
 class SharedMasterBoardData
 {
 public:
@@ -110,4 +110,6 @@ public:
 
   static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
 };
+
+}  // namespace primary_interface
 }  // namespace ur_driver

include/ur_rtde_driver/primary/robot_state/robot_mode_data.h

@@ -0,0 +1,120 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+// Copyright 2015, 2016 Thomas Timm Andersen (original version)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-08
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIBVER_ROBOT_MODE_DATA_H_INCLUDED
+#define UR_RTDE_DRIBVER_ROBOT_MODE_DATA_H_INCLUDED
+
+#include "ur_rtde_driver/primary/robot_state.h"
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+class SharedRobotModeData
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+
+  uint64_t timestamp;
+  bool physical_robot_connected;
+  bool real_robot_enabled;
+  bool robot_power_on;
+  bool emergency_stopped;
+  bool protective_stopped;  // AKA security_stopped
+  bool program_running;
+  bool program_paused;
+
+  static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
+};
+
+enum class robot_mode_V3_X : uint8_t
+{
+  DISCONNECTED = 0,
+  CONFIRM_SAFETY = 1,
+  BOOTING = 2,
+  POWER_OFF = 3,
+  POWER_ON = 4,
+  IDLE = 5,
+  BACKDRIVE = 6,
+  RUNNING = 7,
+  UPDATING_FIRMWARE = 8
+};
+
+enum class robot_control_mode_V3_X : uint8_t
+{
+  POSITION = 0,
+  TEACH = 1,
+  FORCE = 2,
+  TORQUE = 3
+};
+
+class RobotModeData_V3_0__1 : public SharedRobotModeData, public RobotState
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+
+  robot_mode_V3_X robot_mode;
+  robot_control_mode_V3_X control_mode;
+
+  double target_speed_fraction;
+  double speed_scaling;
+
+  static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V3_X) +
+                             sizeof(robot_control_mode_V3_X) + sizeof(double) + sizeof(double);
+
+  static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
+};
+
+class RobotModeData_V3_2 : public RobotModeData_V3_0__1
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  double target_speed_fraction_limit;
+
+  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");
+};
+
+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");
+};
+}  // namespace primary_interface
+}  // namespace ur_driver
+#endif  // ifndef UR_RTDE_DRIBVER_ROBOT_MODE_DATA_H_INCLUDED

include/ur_rtde_driver/types.h

@@ -19,14 +19,21 @@
 #pragma once
 
 #include <inttypes.h>
+#include <array>
 
 namespace ur_driver
 {
+// TODO:Replace
 struct double3_t
 {
   double x, y, z;
 };
 
+using vector3d_t = std::array<double, 3>;
+using vector6d_t = std::array<double, 6>;
+using vector6int32_t = std::array<int32_t, 6>;
+using vector6uint32_t = std::array<uint32_t, 6>;
+
 struct cartesian_coord_t
 {
   double3_t position;

include/ur_rtde_driver/ur/robot_mode.h

@@ -20,9 +20,9 @@
 
 #include <inttypes.h>
 #include <cstddef>
-#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/comm/bin_parser.h"
 #include "ur_rtde_driver/types.h"
-#include "ur_rtde_driver/ur/state.h"
+#include "ur_rtde_driver/comm/ur/state.h"
 
 namespace ur_driver
 {

include/ur_rtde_driver/ur/rt_state.h

@@ -20,8 +20,8 @@
 
 #include <inttypes.h>
 #include <cstddef>
-#include "ur_rtde_driver/bin_parser.h"
-#include "ur_rtde_driver/comm/pipeline.h"
+#include "ur_rtde_driver/comm/bin_parser.h"
+#include "ur_rtde_driver/comm/comm/pipeline.h"
 #include "ur_rtde_driver/types.h"
 
 namespace ur_driver

src/primary/robot_message/version_message.cpp

@@ -1,7 +1,10 @@
 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
 
 // -- BEGIN LICENSE BLOCK ----------------------------------------------
-// Copyright 2019 FZI Forschungszentrum Informatik
+// Copyright 2019 FZI Forschungszentrum Informatik (ur_rtde_driver)
+// Copyright 2017, 2018 Simon Rasmussen (refactor)
+//
+// Copyright 2015, 2016 Thomas Timm Andersen (original version)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -19,33 +22,42 @@
 //----------------------------------------------------------------------
 /*!\file
  *
- * \author  Lea Steffen steffen@fzi.de
- * \date    2019-04-01
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-08
  *
  */
 //----------------------------------------------------------------------
 
-#ifndef UR_RTDE_DRIVER_ROBOT_MODE_DATA_H_INCLUDED
-#define UR_RTDE_DRIVER_ROBOT_MODE_DATA_H_INCLUDED
-
-#include "ur_rtde_driver/primary/primary_package.h"
-#include "ur_rtde_driver/primary/robot_state.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/primary/robot_message/version_message.h"
 
 namespace ur_driver
 {
 namespace primary_interface
 {
-class RobotModeData : PrimaryPackage
+bool VersionMessage::parseWith(comm::BinParser& bp)
 {
-private:
-  RobotState robot_state_;
+  bp.parse(timestamp_);
+  bp.parse(source_);
+  bp.parse(message_type_);
+  bp.parse(project_name_);
+  bp.parse(major_version_);
+  bp.parse(minor_version_);
+  bp.parse(svn_version_);
+  bp.parse(build_number_);
+  bp.parseRemainder(build_date_);
 
-public:
-  RobotModeData() = default;
-  virtual ~RobotModeData() = default;
-};
+  return true;  // not really possible to check dynamic size packets
+}
 
+std::string VersionMessage::toString() const
+{
+  std::stringstream ss;
+  ss << "project name: " << project_name_ << std::endl;
+  ss << "version: " << major_version_ << "." << minor_version_ << "." << svn_version_ << std::endl;
+  ss << "build date: " << build_date_;
+
+  return ss.str();
+}
 }  // namespace primary_interface
 }  // namespace ur_driver
-
-#endif /* UR_RTDE_DRIVER_ROBOT_MODE_DATA_H_INCLUDED */

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

src/ur/messages.cpp

@@ -28,7 +28,7 @@ bool VersionMessage::parseWith(BinParser& bp)
   bp.parse(minor_version);
   bp.parse(svn_version);
   bp.consume(sizeof(uint32_t));  // undocumented field??
-  bp.parse_remainder(build_date);
+  bp.parseRemainder(build_date);
 
   return true;  // not really possible to check dynamic size packets
 }

tests/ur/master_board.cpp

@@ -14,13 +14,13 @@
  * limitations under the License.
  */
 
-#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/comm/ur/master_board.h"
 #include <gtest/gtest.h>
-#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/comm/bin_parser.h"
 #include "ur_rtde_driver/log.h"
-#include "ur_rtde_driver/test/random_data.h"
-#include "ur_rtde_driver/test/utils.h"
-#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/comm/test/random_data.h"
+#include "ur_rtde_driver/comm/test/utils.h"
+#include "ur_rtde_driver/comm/types.h"
 
 using namespace ur_driver;
 

tests/ur/robot_mode.cpp

@@ -14,13 +14,13 @@
  * limitations under the License.
  */
 
-#include "ur_rtde_driver/ur/robot_mode.h"
+#include "ur_rtde_driver/comm/ur/robot_mode.h"
 #include <gtest/gtest.h>
-#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/comm/bin_parser.h"
 #include "ur_rtde_driver/log.h"
-#include "ur_rtde_driver/test/random_data.h"
-#include "ur_rtde_driver/test/utils.h"
-#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/comm/test/random_data.h"
+#include "ur_rtde_driver/comm/test/utils.h"
+#include "ur_rtde_driver/comm/types.h"
 
 using namespace ur_driver;
 TEST(RobotModeData_V1_X, testRandomDataParsing)

tests/ur/rt_state.cpp

@@ -14,13 +14,13 @@
  * limitations under the License.
  */
 
-#include "ur_rtde_driver/ur/rt_state.h"
+#include "ur_rtde_driver/comm/ur/rt_state.h"
 #include <gtest/gtest.h>
-#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/comm/bin_parser.h"
 #include "ur_rtde_driver/log.h"
-#include "ur_rtde_driver/test/random_data.h"
-#include "ur_rtde_driver/test/utils.h"
-#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/comm/test/random_data.h"
+#include "ur_rtde_driver/comm/test/utils.h"
+#include "ur_rtde_driver/comm/types.h"
 
 using namespace ur_driver;