Implemented RTDEParser

CMakeLists.txt

@@ -82,14 +82,15 @@ add_library(ur_rtde_driver
     src/primary/robot_message.cpp
     src/primary/robot_message/version_message.cpp
     src/primary/robot_state/kinematics_info.cpp
-    src/rtde/data_package.cpp
-    src/rtde/request_protocol_version.cpp
-    src/rtde/get_urcontrol_version.cpp
-    src/rtde/text_message.cpp
-    src/rtde/control_package_setup_outputs.cpp
-    src/rtde/control_package_setup_inputs.cpp
-    src/rtde/control_package_start.cpp
     src/rtde/control_package_pause.cpp
+    src/rtde/control_package_setup_inputs.cpp
+    src/rtde/control_package_setup_outputs.cpp
+    src/rtde/control_package_start.cpp
+    src/rtde/data_package.cpp
+    src/rtde/get_urcontrol_version.cpp
+    src/rtde/request_protocol_version.cpp
+    src/rtde/rtde_package.cpp
+    src/rtde/text_message.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/rtde/rtde_package.h

@@ -46,7 +46,16 @@ public:
    * \brief Creates a new RTDEPackage object.
    */
   RTDEPackage() = default;
+  RTDEPackage(const PackageType type) : type_(type)
+  {
+  }
   virtual ~RTDEPackage() = default;
+
+  virtual bool parseWith(comm::BinParser& bp);
+  virtual std::string toString() const;
+
+protected:
+  PackageType type_;
 };
 
 }  // namespace rtde_interface

include/ur_rtde_driver/rtde/rtde_parser.h

@@ -21,39 +21,89 @@
 #include "ur_rtde_driver/comm/parser.h"
 #include "ur_rtde_driver/comm/bin_parser.h"
 #include "ur_rtde_driver/comm/pipeline.h"
+
 #include "ur_rtde_driver/rtde/package_header.h"
+#include "ur_rtde_driver/rtde/data_package.h"
+#include "ur_rtde_driver/rtde/text_message.h"
+#include "ur_rtde_driver/rtde/request_protocol_version.h"
+#include "ur_rtde_driver/rtde/get_urcontrol_version.h"
 
 namespace ur_driver
 {
 namespace rtde_interface
 {
-using namespace comm;
 template <typename T>
 class RTDEParser : comm::Parser<rtde_interface::PackageHeader>
 {
 public:
-  bool parse(BinParser& bp, std::vector<std::unique_ptr<PackageHeader>>& results)
+  RTDEParser() = default;
+  virtual ~RTDEParser() = default;
+
+  bool parse(comm::BinParser& bp, std::vector<std::unique_ptr<comm::URPackage<PackageHeader>>>& results)
 
   {
-    int32_t packet_size = bp.peek<int32_t>();
+    PackageHeader::_package_size_type size;
+    PackageType type;
+    bp.parse(size);
+    bp.parse(type);
 
-    if (!bp.checkSize(packet_size))
+    if (!bp.checkSize(size))
     {
       LOG_ERROR("Buffer len shorter than expected packet length");
       return false;
     }
 
-    bp.parse(packet_size);  // consumes the peeked data
+    switch (type)
+    {
+      case PackageType::RTDE_DATA_PACKAGE:
+      {
+        // TODO: get proper recipe here
+        std::vector<std::string> recipe;
+        std::unique_ptr<RTDEPackage> package(new DataPackage(recipe));
 
-    std::unique_ptr<PackageHeader> packet(new T);
+        if (!package->parseWith(bp))
-
+        {
-    if (!packet->parseWith(bp))
           return false;
+        }
+        results.push_back(std::move(package));
+        break;
+      }
+      default:
+      {
+        std::unique_ptr<RTDEPackage> package(package_from_type(type));
+        if (!package->parseWith(bp))
+        {
+          LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type));
+          return false;
+        }
+
+        results.push_back(std::move(package));
+        break;
+      }
+    }
 
-    results.push_back(std::move(packet));
 
     return true;
   }
+
+private:
+  RTDEPackage* package_from_type(PackageType type)
+  {
+    switch (type)
+    {
+      case PackageType::RTDE_TEXT_MESSAGE:
+        return new TextMessage;
+        break;
+      case PackageType::RTDE_GET_URCONTROL_VERSION:
+        return new GetUrcontrolVersion;
+        break;
+      case PackageType::RTDE_REQUEST_PROTOCOL_VERSION:
+        return new RequestProtocolVersion;
+        break;
+      default:
+        return new RTDEPackage(type);
+    }
+  }
 };
 
 }  // namespace rtde_interface

src/rtde/rtde_package.cpp

@@ -0,0 +1,45 @@
+// 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-10
+ *
+ */
+//----------------------------------------------------------------------
+#include "ur_rtde_driver/rtde/rtde_package.h"
+
+namespace ur_driver
+{
+namespace rtde_interface
+{
+bool RTDEPackage::parseWith(comm::BinParser& bp)
+{
+  return true;
+}
+
+std::string rtde_interface::RTDEPackage ::toString() const
+{
+  std::stringstream ss;
+  ss << "Type: " << static_cast<int>(type_);
+  return ss.str();
+}
+}  // namespace rtde_interface
+}  // namespace ur_driver