renamed package

include/ur_rtde_driver/ur/commander.h

@@ -0,0 +1,93 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <array>
+#include <iomanip>
+#include <sstream>
+#include "ur_rtde_driver/ur/stream.h"
+
+class URCommander
+{
+private:
+  URStream &stream_;
+
+protected:
+  bool write(const std::string &s);
+  void formatArray(std::ostringstream &out, std::array<double, 6> &values);
+
+public:
+  URCommander(URStream &stream) : stream_(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0;
+  virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
+  virtual bool setAnalogOut(uint8_t pin, double value) = 0;
+
+  // shared
+  bool uploadProg(const std::string &s);
+  bool stopj(double a = 10.0);
+  bool setToolVoltage(uint8_t voltage);
+  bool setFlag(uint8_t pin, bool value);
+  bool setPayload(double value);
+};
+
+class URCommander_V1_X : public URCommander
+{
+public:
+  URCommander_V1_X(URStream &stream) : URCommander(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+  virtual bool setDigitalOut(uint8_t pin, bool value);
+  virtual bool setAnalogOut(uint8_t pin, double value);
+};
+
+class URCommander_V3_X : public URCommander
+{
+public:
+  URCommander_V3_X(URStream &stream) : URCommander(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration) = 0;
+  virtual bool setDigitalOut(uint8_t pin, bool value);
+  virtual bool setAnalogOut(uint8_t pin, double value);
+};
+
+class URCommander_V3_1__2 : public URCommander_V3_X
+{
+public:
+  URCommander_V3_1__2(URStream &stream) : URCommander_V3_X(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+};
+
+class URCommander_V3_3 : public URCommander_V3_X
+{
+public:
+  URCommander_V3_3(URStream &stream) : URCommander_V3_X(stream)
+  {
+  }
+
+  virtual bool speedj(std::array<double, 6> &speeds, double acceleration);
+};

include/ur_rtde_driver/ur/consumer.h

@@ -0,0 +1,68 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/ur/messages.h"
+#include "ur_rtde_driver/ur/robot_mode.h"
+#include "ur_rtde_driver/ur/rt_state.h"
+#include "ur_rtde_driver/ur/state.h"
+
+class URRTPacketConsumer : public IConsumer<RTPacket>
+{
+public:
+  virtual bool consume(shared_ptr<RTPacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
+
+  virtual bool consume(RTState_V1_6__7& state) = 0;
+  virtual bool consume(RTState_V1_8& state) = 0;
+  virtual bool consume(RTState_V3_0__1& state) = 0;
+  virtual bool consume(RTState_V3_2__3& state) = 0;
+};
+
+class URStatePacketConsumer : public IConsumer<StatePacket>
+{
+public:
+  virtual bool consume(shared_ptr<StatePacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
+
+  virtual bool consume(MasterBoardData_V1_X& data) = 0;
+  virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
+  virtual bool consume(MasterBoardData_V3_2& data) = 0;
+
+  virtual bool consume(RobotModeData_V1_X& data) = 0;
+  virtual bool consume(RobotModeData_V3_0__1& data) = 0;
+  virtual bool consume(RobotModeData_V3_2& data) = 0;
+};
+
+class URMessagePacketConsumer : public IConsumer<MessagePacket>
+{
+public:
+  virtual bool consume(shared_ptr<MessagePacket> packet)
+  {
+    return packet->consumeWith(*this);
+  }
+
+  virtual bool consume(VersionMessage& message) = 0;
+};

include/ur_rtde_driver/ur/factory.h

@@ -0,0 +1,141 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#include <cstdlib>
+#include "ur_rtde_driver/ur/consumer.h"
+#include "ur_rtde_driver/ur/messages_parser.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/producer.h"
+#include "ur_rtde_driver/ur/rt_parser.h"
+#include "ur_rtde_driver/ur/state_parser.h"
+#include "ur_rtde_driver/ur/stream.h"
+
+static const int UR_PRIMARY_PORT = 30001;
+
+class URFactory : private URMessagePacketConsumer
+{
+private:
+  URStream stream_;
+  URMessageParser parser_;
+
+  uint8_t major_version_;
+  uint8_t minor_version_;
+
+  bool consume(VersionMessage& vm)
+  {
+    LOG_INFO("Got VersionMessage:");
+    LOG_INFO("project name: %s", vm.project_name.c_str());
+    LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
+    LOG_INFO("build date: %s", vm.build_date.c_str());
+
+    major_version_ = vm.major_version;
+    minor_version_ = vm.minor_version;
+
+    return true;
+  }
+
+  void setupConsumer()
+  {
+  }
+  void teardownConsumer()
+  {
+  }
+  void stopConsumer()
+  {
+  }
+
+public:
+  URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT)
+  {
+    URProducer<MessagePacket> prod(stream_, parser_);
+    std::vector<unique_ptr<MessagePacket>> results;
+
+    prod.setupProducer();
+
+    if (!prod.tryGet(results) || results.size() == 0)
+    {
+      LOG_FATAL("No version message received, init failed!");
+      std::exit(EXIT_FAILURE);
+    }
+
+    for (auto const& p : results)
+    {
+      p->consumeWith(*this);
+    }
+
+    if (major_version_ == 0 && minor_version_ == 0)
+    {
+      LOG_FATAL("No version message received, init failed!");
+      std::exit(EXIT_FAILURE);
+    }
+
+    prod.teardownProducer();
+  }
+
+  bool isVersion3()
+  {
+    return major_version_ == 3;
+  }
+
+  std::unique_ptr<URCommander> getCommander(URStream& stream)
+  {
+    if (major_version_ == 1)
+      return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
+    else if (minor_version_ < 3)
+      return std::unique_ptr<URCommander>(new URCommander_V3_1__2(stream));
+    else
+      return std::unique_ptr<URCommander>(new URCommander_V3_3(stream));
+  }
+
+  std::unique_ptr<URParser<StatePacket>> getStateParser()
+  {
+    if (major_version_ == 1)
+    {
+      return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
+    }
+    else
+    {
+      if (minor_version_ < 3)
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1);
+      else if (minor_version_ < 5)
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
+      else
+        return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_5);
+    }
+  }
+
+  std::unique_ptr<URParser<RTPacket>> getRTParser()
+  {
+    if (major_version_ == 1)
+    {
+      if (minor_version_ < 8)
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
+    }
+    else
+    {
+      if (minor_version_ < 3)
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);
+    }
+  }
+};

include/ur_rtde_driver/ur/master_board.h

@@ -0,0 +1,110 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#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"
+
+class SharedMasterBoardData
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+
+  int8_t analog_input_range0;
+  int8_t analog_input_range1;
+  double analog_input0;
+  double analog_input1;
+  int8_t analog_output_domain0;
+  int8_t analog_output_domain1;
+  double analog_output0;
+  double analog_output1;
+  float master_board_temperature;
+  float robot_voltage_48V;
+  float robot_current;
+  float master_IO_current;
+
+  bool euromap67_interface_installed;
+
+  // euromap fields are dynamic so don't include in SIZE
+  int32_t euromap_input_bits;
+  int32_t euromap_output_bits;
+
+  static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
+
+  static const size_t EURO_SIZE = sizeof(int32_t) * 2;
+};
+
+class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  std::bitset<10> digital_input_bits;
+  std::bitset<10> digital_output_bits;
+
+  uint8_t master_safety_state;
+  bool master_on_off_state;
+
+  // euromap fields are dynamic so don't include in SIZE
+  int16_t euromap_voltage;
+  int16_t euromap_current;
+
+  static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
+
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
+};
+
+class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  std::bitset<18> digital_input_bits;
+  std::bitset<18> digital_output_bits;
+
+  uint8_t safety_mode;
+  bool in_reduced_mode;
+
+  // euromap fields are dynamic so don't include in SIZE
+  float euromap_voltage;
+  float euromap_current;
+
+  static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 +
+                             sizeof(uint32_t);  // UR internal field we skip
+
+  static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
+};
+
+class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  uint8_t operational_mode_selector_input;
+  uint8_t three_position_enabling_device_input;
+
+  static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
+};

include/ur_rtde_driver/ur/messages.h

@@ -0,0 +1,69 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/pipeline.h"
+
+enum class robot_message_type : uint8_t
+{
+  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 URMessagePacketConsumer;
+
+class MessagePacket
+{
+public:
+  MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source)
+  {
+  }
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URMessagePacketConsumer& consumer) = 0;
+
+  uint64_t timestamp;
+  uint8_t source;
+};
+
+class VersionMessage : public MessagePacket
+{
+public:
+  VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source)
+  {
+  }
+
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URMessagePacketConsumer& consumer);
+
+  std::string project_name;
+  uint8_t major_version;
+  uint8_t minor_version;
+  int32_t svn_version;
+  std::string build_date;
+};

include/ur_rtde_driver/ur/messages_parser.h

@@ -0,0 +1,71 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <vector>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/messages.h"
+#include "ur_rtde_driver/ur/parser.h"
+
+class URMessageParser : public URParser<MessagePacket>
+{
+public:
+  bool parse(BinParser& bp, std::vector<unique_ptr<MessagePacket>>& results)
+  {
+    int32_t packet_size;
+    message_type type;
+    bp.parse(packet_size);
+    bp.parse(type);
+
+    if (type != message_type::ROBOT_MESSAGE)
+    {
+      LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+      return false;
+    }
+
+    uint64_t timestamp;
+    uint8_t source;
+    robot_message_type message_type;
+
+    bp.parse(timestamp);
+    bp.parse(source);
+    bp.parse(message_type);
+
+    std::unique_ptr<MessagePacket> result;
+    bool parsed = false;
+
+    switch (message_type)
+    {
+      case robot_message_type::ROBOT_MESSAGE_VERSION:
+      {
+        VersionMessage* vm = new VersionMessage(timestamp, source);
+        parsed = vm->parseWith(bp);
+        result.reset(vm);
+        break;
+      }
+
+      default:
+        return false;
+    }
+
+    results.push_back(std::move(result));
+    return parsed;
+  }
+};

include/ur_rtde_driver/ur/parser.h

@@ -0,0 +1,29 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <vector>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/pipeline.h"
+
+template <typename T>
+class URParser
+{
+public:
+  virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
+};

include/ur_rtde_driver/ur/producer.h

@@ -0,0 +1,83 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <chrono>
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/stream.h"
+
+template <typename T>
+class URProducer : public IProducer<T>
+{
+private:
+  URStream& stream_;
+  URParser<T>& parser_;
+  std::chrono::seconds timeout_;
+
+public:
+  URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser), timeout_(1)
+  {
+  }
+
+  void setupProducer()
+  {
+    stream_.connect();
+  }
+  void teardownProducer()
+  {
+    stream_.disconnect();
+  }
+  void stopProducer()
+  {
+    stream_.disconnect();
+  }
+
+  bool tryGet(std::vector<unique_ptr<T>>& products)
+  {
+    // 4KB should be enough to hold any packet received from UR
+    uint8_t buf[4096];
+    size_t read = 0;
+    // expoential backoff reconnects
+    while (true)
+    {
+      if (stream_.read(buf, sizeof(buf), read))
+      {
+        // reset sleep amount
+        timeout_ = std::chrono::seconds(1);
+        break;
+      }
+
+      if (stream_.closed())
+        return false;
+
+      LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
+      std::this_thread::sleep_for(timeout_);
+
+      if (stream_.connect())
+        continue;
+
+      auto next = timeout_ * 2;
+      if (next <= std::chrono::seconds(120))
+        timeout_ = next;
+    }
+
+    BinParser bp(buf, read);
+    return parser_.parse(bp, products);
+  }
+};

include/ur_rtde_driver/ur/robot_mode.h

@@ -0,0 +1,136 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/ur/state.h"
+
+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_V1_X : uint8_t
+{
+  ROBOT_RUNNING_MODE = 0,
+  ROBOT_FREEDRIVE_MODE = 1,
+  ROBOT_READY_MODE = 2,
+  ROBOT_INITIALIZING_MODE = 3,
+  ROBOT_SECURITY_STOPPED_MODE = 4,
+  ROBOT_EMERGENCY_STOPPED_MODE = 5,
+  ROBOT_FATAL_ERROR_MODE = 6,
+  ROBOT_NO_POWER_MODE = 7,
+  ROBOT_NOT_CONNECTED_MODE = 8,
+  ROBOT_SHUTDOWN_MODE = 9,
+  ROBOT_SAFEGUARD_STOP_MODE = 10
+};
+
+class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  robot_mode_V1_X robot_mode;
+  double speed_fraction;
+
+  static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double);
+
+  static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
+};
+
+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 StatePacket
+{
+public:
+  virtual bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URStatePacketConsumer& consumer);
+
+  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");
+};

include/ur_rtde_driver/ur/rt_parser.h

@@ -0,0 +1,56 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <vector>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/rt_state.h"
+
+template <typename T>
+class URRTStateParser : public URParser<RTPacket>
+{
+public:
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket>>& results)
+  {
+    int32_t packet_size = bp.peek<int32_t>();
+
+    if (!bp.checkSize(packet_size))
+    {
+      LOG_ERROR("Buffer len shorter than expected packet length");
+      return false;
+    }
+
+    bp.parse(packet_size);  // consumes the peeked data
+
+    std::unique_ptr<RTPacket> packet(new T);
+    if (!packet->parseWith(bp))
+      return false;
+
+    results.push_back(std::move(packet));
+
+    return true;
+  }
+};
+
+typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;
+typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8;
+typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1;
+typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;

include/ur_rtde_driver/ur/rt_state.h

@@ -0,0 +1,137 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/types.h"
+
+class URRTPacketConsumer;
+
+class RTPacket
+{
+public:
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URRTPacketConsumer& consumer) = 0;
+};
+
+class RTShared
+{
+protected:
+  void parse_shared1(BinParser& bp);
+  void parse_shared2(BinParser& bp);
+
+public:
+  double time;
+  std::array<double, 6> q_target;
+  std::array<double, 6> qd_target;
+  std::array<double, 6> qdd_target;
+  std::array<double, 6> i_target;
+  std::array<double, 6> m_target;
+  std::array<double, 6> q_actual;
+  std::array<double, 6> qd_actual;
+  std::array<double, 6> i_actual;
+
+  // gap here depending on version
+
+  std::array<double, 6> tcp_force;
+
+  // does not contain "_actual" postfix in V1_X but
+  // they're the same fields so share anyway
+  cartesian_coord_t tool_vector_actual;
+  cartesian_coord_t tcp_speed_actual;
+
+  // gap here depending on version
+
+  uint64_t digital_inputs;
+  std::array<double, 6> motor_temperatures;
+  double controller_time;
+  double robot_mode;
+
+  static const size_t SIZE =
+      sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t);
+};
+
+class RTState_V1_6__7 : public RTShared, public RTPacket
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  double3_t tool_accelerometer_values;
+
+  static const size_t SIZE = RTShared::SIZE + sizeof(double3_t);
+
+  static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
+};
+
+class RTState_V1_8 : public RTState_V1_6__7
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  std::array<double, 6> joint_modes;
+
+  static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]);
+
+  static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
+};
+
+class RTState_V3_0__1 : public RTShared, public RTPacket
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  std::array<double, 6> i_control;
+  cartesian_coord_t tool_vector_target;
+  cartesian_coord_t tcp_speed_target;
+
+  std::array<double, 6> joint_modes;
+  double safety_mode;
+  double3_t tool_accelerometer_values;
+  double speed_scaling;
+  double linear_momentum_norm;
+  double v_main;
+  double v_robot;
+  double i_robot;
+  std::array<double, 6> v_actual;
+
+  static const size_t SIZE =
+      RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6;
+
+  static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
+};
+
+class RTState_V3_2__3 : public RTState_V3_0__1
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  uint64_t digital_outputs;
+  double program_state;
+
+  static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double);
+
+  static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
+};

include/ur_rtde_driver/ur/server.h

@@ -0,0 +1,49 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <atomic>
+#include <cstdlib>
+#include <mutex>
+#include <string>
+#include "ur_rtde_driver/tcp_socket.h"
+
+#define MAX_SERVER_BUF_LEN 50
+
+class URServer : private TCPSocket
+{
+private:
+  int port_;
+  TCPSocket client_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len);
+
+public:
+  URServer(int port);
+  ~URServer();
+  std::string getIP();
+  bool bind();
+  bool accept();
+  void disconnectClient();
+  bool readLine(char *buffer, size_t buf_len);
+  bool write(const uint8_t *buf, size_t buf_len, size_t &written);
+};

include/ur_rtde_driver/ur/state.h

@@ -0,0 +1,61 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <cstddef>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/pipeline.h"
+
+enum class package_type : uint8_t
+{
+  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
+};
+
+enum class message_type : uint8_t
+{
+  ROBOT_STATE = 16,
+  ROBOT_MESSAGE = 20,
+  PROGRAM_STATE_MESSAGE = 25
+};
+
+class URStatePacketConsumer;
+
+class StatePacket
+{
+public:
+  StatePacket()
+  {
+  }
+  virtual ~StatePacket()
+  {
+  }
+  virtual bool parseWith(BinParser& bp) = 0;
+  virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
+};

include/ur_rtde_driver/ur/state_parser.h

@@ -0,0 +1,117 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <vector>
+#include "ur_rtde_driver/bin_parser.h"
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/pipeline.h"
+#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/ur/parser.h"
+#include "ur_rtde_driver/ur/robot_mode.h"
+#include "ur_rtde_driver/ur/state.h"
+
+template <typename RMD, typename MBD>
+class URStateParser : public URParser<StatePacket>
+{
+private:
+  StatePacket* from_type(package_type type)
+  {
+    switch (type)
+    {
+      case package_type::ROBOT_MODE_DATA:
+        return new RMD;
+      case package_type::MASTERBOARD_DATA:
+        return new MBD;
+      default:
+        return nullptr;
+    }
+  }
+
+public:
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket>>& results)
+  {
+    int32_t packet_size;
+    message_type type;
+    bp.parse(packet_size);
+    bp.parse(type);
+
+    if (type != message_type::ROBOT_STATE)
+    {
+      // quietly ignore the intial version message
+      if (type != message_type::ROBOT_MESSAGE)
+      {
+        LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
+      }
+
+      bp.consume();
+      return true;
+    }
+
+    while (!bp.empty())
+    {
+      if (!bp.checkSize(sizeof(uint32_t)))
+      {
+        LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
+        return false;
+      }
+      uint32_t sub_size = bp.peek<uint32_t>();
+      if (!bp.checkSize(static_cast<size_t>(sub_size)))
+      {
+        LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
+        return false;
+      }
+
+      // deconstruction of a sub parser will increment the position of the parent parser
+      BinParser sbp(bp, sub_size);
+      sbp.consume(sizeof(sub_size));
+      package_type type;
+      sbp.parse(type);
+
+      std::unique_ptr<StatePacket> packet(from_type(type));
+
+      if (packet == nullptr)
+      {
+        sbp.consume();
+        continue;
+      }
+
+      if (!packet->parseWith(sbp))
+      {
+        LOG_ERROR("Sub-package parsing of type %d failed!", static_cast<int>(type));
+        return false;
+      }
+
+      results.push_back(std::move(packet));
+
+      if (!sbp.empty())
+      {
+        LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast<int>(type));
+        sbp.debug();
+        return false;
+      }
+    }
+
+    return true;
+  }
+};
+
+typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
+typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
+typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
+typedef URStateParser<RobotModeData_V3_5, MasterBoardData_V3_2> URStateParser_V3_5;

include/ur_rtde_driver/ur/stream.h

@@ -0,0 +1,64 @@
+/*
+ * 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.
+ * 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.
+ */
+
+#pragma once
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <atomic>
+#include <mutex>
+#include <string>
+#include "ur_rtde_driver/log.h"
+#include "ur_rtde_driver/tcp_socket.h"
+
+class URStream : public TCPSocket
+{
+private:
+  std::string host_;
+  int port_;
+  std::mutex write_mutex_, read_mutex_;
+
+protected:
+  virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
+  {
+    return ::connect(socket_fd, address, address_len) == 0;
+  }
+
+public:
+  URStream(std::string& host, int port) : host_(host), port_(port)
+  {
+  }
+
+  bool connect()
+  {
+    return TCPSocket::setup(host_, port_);
+  }
+  void disconnect()
+  {
+    LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
+    TCPSocket::close();
+  }
+
+  bool closed()
+  {
+    return getState() == SocketState::Closed;
+  }
+
+  bool read(uint8_t* buf, size_t buf_len, size_t& read);
+  bool write(const uint8_t* buf, size_t buf_len, size_t& written);
+};