Added parse and toString functions for higher-level primary functions

CMakeLists.txt

@@ -75,6 +75,8 @@ add_library(ur_rtde_driver
     #src/ur/robot_mode.cpp
     #src/ur/rt_state.cpp
     #src/ur/server.cpp
+    src/primary/primary_package.cpp
+    src/primary/robot_message.cpp
     src/primary/robot_message/version_message.cpp
     src/primary/robot_state/kinematics_info.cpp
 )

include/ur_rtde_driver/primary/primary_package.h

@@ -35,16 +35,20 @@ namespace ur_driver
 {
 namespace primary_interface
 {
-class PrimaryPackage : comm::URPackage<PackageHeader>
+class PrimaryPackage : public comm::URPackage<PackageHeader>
 {
 public:
   PrimaryPackage() = default;
   virtual ~PrimaryPackage() = default;
 
+  virtual bool parseWith(comm::BinParser& bp);
+  virtual std::string toString() const;
+
 private:
-  uint8_t* data_buffer_;
+  std::string buffer_;
 };
 
+
 }  // namespace primary_interface
 }  // namespace ur_driver
 

include/ur_rtde_driver/primary/robot_message.h

@@ -34,20 +34,7 @@ namespace ur_driver
 {
 namespace primary_interface
 {
-enum class message_type : uint8_t
+class RobotMessage : public PrimaryPackage
-{
-  ROBOT_MESSAGE_TEXT = 0,
-  ROBOT_MESSAGE_PROGRAM_LABEL = 1,
-  PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
-  ROBOT_MESSAGE_VERSION = 3,
-  ROBOT_MESSAGE_SAFETY_MODE = 5,
-  ROBOT_MESSAGE_ERROR_CODE = 6,
-  ROBOT_MESSAGE_KEY = 7,
-  ROBOT_MESSAGE_REQUEST_VALUE = 9,
-  ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
-};
-
-class RobotMessage : PrimaryPackage
 {
 public:
   RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source)
@@ -55,8 +42,8 @@ public:
   }
   virtual ~RobotMessage() = default;
 
-  virtual bool parseWith(comm::BinParser& bp) = 0;
+  virtual bool parseWith(comm::BinParser& bp);
-  virtual std::string toString() const = 0;
+  virtual std::string toString() const;
 
   uint64_t timestamp_;
   uint8_t source_;

include/ur_rtde_driver/primary/robot_message/version_message.h

@@ -40,6 +40,7 @@ public:
   VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source)
   {
   }
+  virtual ~VersionMessage() = default;
 
   virtual bool parseWith(comm::BinParser& bp);
 

include/ur_rtde_driver/primary/robot_state.h

@@ -48,6 +48,15 @@ public:
   RobotState() = default;
   virtual ~RobotState() = default;
 
+  virtual bool parseWith(comm::BinParser& bp)
+  {
+    return true;
+  }
+  virtual std::string toString() const
+  {
+    return std::string();
+  }
+
 private:
 };
 

include/ur_rtde_driver/primary/robot_state/kinematics_info.h

@@ -26,7 +26,7 @@ namespace primary_interface
  * a combination between the perfect model parameters and the correction deltas as noted in the
  * configuration files on the robot controller.
  */
-class KinematicsInfo : RobotState
+class KinematicsInfo : public RobotState
 {
 public:
   KinematicsInfo() = default;

include/ur_rtde_driver/rtde/package_header.h

@@ -43,6 +43,9 @@ public:
   virtual ~PackageHeader() = default;
   using _package_size_type = uint16_t;
 
+  PackageHeader(PackageType& type) : package_type_(type){};
+  PackageHeader(_package_size_type& size, PackageType& type) : package_size_(size), package_type_(type){};
+
   static size_t getPackageLength(uint8_t* buf)
   {
     return be16toh(*(reinterpret_cast<_package_size_type*>(buf)));

src/primary/primary_package.cpp

@@ -0,0 +1,46 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// 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-09
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/primary/primary_package.h"
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+bool PrimaryPackage::parseWith(comm::BinParser& bp)
+{
+  bp.parseRemainder(buffer_);
+  return true;
+}
+
+std::string PrimaryPackage::toString() const
+{
+  return buffer_;
+}
+
+}  // namespace primary_interface
+}  // namespace ur_driver

src/primary/robot_message.cpp

@@ -0,0 +1,53 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik (ur_rtde_driver)
+//
+// 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-09
+ *
+ */
+//----------------------------------------------------------------------
+#include "ur_rtde_driver/primary/robot_message.h"
+
+namespace ur_driver
+{
+namespace primary_interface
+{
+bool RobotMessage::parseWith(comm::BinParser& bp)
+{
+  bp.parse(timestamp_);
+  bp.parse(source_);
+  bp.parse(message_type_);
+
+  return true;
+}
+
+std::string RobotMessage::toString() const
+{
+  std::stringstream ss;
+  ss << "timestamp: " << timestamp_ << std::endl;
+  ss << "source: " << static_cast<int>(source_) << std::endl;
+  ss << "message_type: " << static_cast<int>(message_type_) << std::endl;
+
+  return ss.str();
+}
+
+}  // namespace primary_interface
+}  // namespace ur_driver

src/primary/robot_message/version_message.cpp

@@ -37,9 +37,7 @@ namespace primary_interface
 {
 bool VersionMessage::parseWith(comm::BinParser& bp)
 {
-  bp.parse(timestamp_);
+  RobotMessage::parseWith(bp);
-  bp.parse(source_);
-  bp.parse(message_type_);
   bp.parse(project_name_);
   bp.parse(major_version_);
   bp.parse(minor_version_);