added the rtde client class that initializes the rtde handshake and

allows reading of data packages

CMakeLists.txt

@@ -91,6 +91,7 @@ add_library(ur_rtde_driver
     src/rtde/request_protocol_version.cpp
     src/rtde/rtde_package.cpp
     src/rtde/text_message.cpp
+    src/rtde/rtde_client.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/pipeline.h

@@ -181,9 +181,9 @@ public:
     notifier_.stopped(name_);
   }
 
-  bool getLatestProduct(std::unique_ptr<URPackage<HeaderT>> product, std::chrono::milliseconds timeout)
+  bool getLatestProduct(std::unique_ptr<URPackage<HeaderT>>& product, std::chrono::milliseconds timeout)
   {
-    return !queue_.wait_dequeue_timed(product, timeout);
+    return queue_.wait_dequeue_timed(product, timeout);
   }
 
 private:

include/ur_rtde_driver/rtde/rtde_client.h

@@ -0,0 +1,65 @@
+// 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  Tristan Schnell schnell@fzi.de
+ * \date    2019-04-10
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED
+#define UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED
+
+#include "ur_rtde_driver/comm/pipeline.h"
+#include "ur_rtde_driver/rtde/package_header.h"
+#include "ur_rtde_driver/rtde/rtde_package.h"
+#include "ur_rtde_driver/comm/stream.h"
+#include "ur_rtde_driver/rtde/rtde_parser.h"
+#include "ur_rtde_driver/comm/producer.h"
+#include "ur_rtde_driver/rtde/data_package.h"
+#include "ur_rtde_driver/rtde/request_protocol_version.h"
+
+static const int UR_RTDE_PORT = 30004;
+static const std::string PIPELINE_NAME = "RTDE Data Pipeline";
+
+namespace ur_driver
+{
+namespace rtde_interface
+{
+class RTDEClient
+{
+public:
+  RTDEClient() = delete;
+  RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier);
+  ~RTDEClient() = default;
+  bool getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package, std::chrono::milliseconds timeout);
+
+private:
+  comm::URStream<PackageHeader> stream_;
+  RTDEParser parser_;
+  comm::URProducer<PackageHeader> prod_;
+  comm::Pipeline<PackageHeader> pipeline_;
+};
+
+}  // namespace rtde_interface
+}  // namespace ur_driver
+
+#endif  // UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED

src/rtde/rtde_client.cpp

@@ -0,0 +1,64 @@
+// 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  Tristan Schnell schnell@fzi.de
+ * \date    2019-04-10
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/rtde/rtde_client.h"
+
+namespace ur_driver
+{
+namespace rtde_interface
+{
+RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier)
+    : stream_(ROBOT_IP, UR_RTDE_PORT), parser_(), prod_(stream_, parser_),
+      pipeline_(prod_, PIPELINE_NAME, notifier)
+{
+  pipeline_.run();
+
+  sleep(1);
+
+  uint8_t buffer[4096];
+  size_t size;
+  size_t written;
+  size = RequestProtocolVersionRequest::generateSerializedRequest(buffer);
+  std::cout << "size: " << size << std::endl;
+  std::cout << "buffer: " << static_cast<int>(buffer[0]) << " - "
+            << static_cast<int>(buffer[1]) << " - "
+            << static_cast<int>(buffer[2]) << " - "
+            << static_cast<int>(buffer[3]) << " - "
+            << static_cast<int>(buffer[4]) << std::endl;
+   stream_.write(buffer, size, written);
+
+  std::cout << "wrote: " << written << std::endl;
+}
+
+bool RTDEClient::getDataPackage(
+    std::unique_ptr<comm::URPackage<PackageHeader>>& data_package,
+    std::chrono::milliseconds timeout)
+{
+  return pipeline_.getLatestProduct(data_package, timeout);
+}
+} // namespace rtde_interface
+} // namespace ur_driver