Completed parsing of UR state, messages and RT

src/ur/master_board.cpp

@@ -1,4 +1,5 @@
 #include "ur_modern_driver/ur/master_board.h"
+#include "ur_modern_driver/ur/consumer.h"
 
 bool SharedMasterBoardData::parse_with(BinParser &bp) {
     bp.parse(analog_input_range0);
@@ -80,3 +81,16 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
     return true;
 }
 
+
+
+
+
+bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}

src/ur/messages.cpp

@@ -1,11 +1,5 @@
 #include "ur_modern_driver/ur/messages.h"
-
-bool MessageBase::parse_with(BinParser &bp) {
-    bp.parse(timestamp);
-    bp.parse(source);
-
-    return true; //not really possible to check dynamic size packets
-}
+#include "ur_modern_driver/ur/consumer.h"
 
 
 bool VersionMessage::parse_with(BinParser &bp) {
@@ -18,4 +12,9 @@ bool VersionMessage::parse_with(BinParser &bp) {
     bp.parse_remainder(build_date);
 
     return true; //not really possible to check dynamic size packets    
+}
+
+
+bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) {
+    return consumer.consume(*this);
 }

src/ur/producer.cpp

@@ -1,32 +0,0 @@
-#include "ur_modern_driver/ur/producer.h"
-#include "ur_modern_driver/log.h"
-
-void URProducer::setup_producer() {
-    _stream.connect();
-}
-
-void URProducer::teardown_producer() {
-
-}
-
-void URProducer::stop_producer() {
-    _stream.disconnect();
-}
-
-std::unique_ptr<Packet> URProducer::try_get() {
-    //4KB should be enough to hold any packet received from UR
-    uint8_t buf[4096];
-
-    //blocking call
-    ssize_t len = _stream.receive(buf, sizeof(buf));
-
-    LOG_DEBUG("Read %d bytes from stream", len);
-
-    if(len < 1) {
-        LOG_WARN("Read nothing from stream");
-        return std::unique_ptr<Packet>(nullptr);
-    }
-
-    BinParser bp(buf, static_cast<size_t>(len));
-    return std::move(_parser.parse(bp));
-}

src/ur/robot_mode.cpp

@@ -1,4 +1,5 @@
 #include "ur_modern_driver/ur/robot_mode.h"
+#include "ur_modern_driver/ur/consumer.h"
 
 
 bool SharedRobotModeData::parse_with(BinParser &bp) {
@@ -54,3 +55,16 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) {
 
     return true;
 }
+
+
+
+
+bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}

src/ur/rt_state.cpp

@@ -1,9 +1,8 @@
 #include "ur_modern_driver/ur/rt_state.h"
+#include "ur_modern_driver/ur/consumer.h"
 
-bool RTState_V1_6__7::parse_with(BinParser &bp) {
-    if(!bp.check_size<RTState_V1_6__7>())
-        return false;
 
+bool RTShared::parse_shared1(BinParser &bp) {
     bp.parse(time);
     bp.parse(q_target);
     bp.parse(qd_target);
@@ -13,14 +12,30 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) {
     bp.parse(q_actual);
     bp.parse(qd_actual);
     bp.parse(i_actual);
-    bp.parse(tool_accelerometer_values);
-    bp.parse(tcp_force);
-    bp.parse(tool_vector);
-    bp.parse(tcp_speed);
+    return true;
+}
+
+bool RTShared::parse_shared2(BinParser &bp) {
     bp.parse(digital_input);
     bp.parse(motor_temperatures);
     bp.parse(controller_time);
     bp.parse(robot_mode);
+    return true;
+}
+
+
+bool RTState_V1_6__7::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V1_6__7>())
+        return false;
+
+    parse_shared1(bp);
+    
+    bp.parse(tool_accelerometer_values);
+    bp.parse(tcp_force);
+    bp.parse(tool_vector_actual);
+    bp.parse(tcp_speed_actual);
+
+    parse_shared2(bp);
 
     return true;
 }
@@ -34,4 +49,61 @@ bool RTState_V1_8::parse_with(BinParser &bp) {
     bp.parse(joint_modes);
 
     return true;
+}
+
+bool RTState_V3_0__1::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V3_0__1>())
+        return false;
+
+    parse_shared1(bp);
+
+    bp.parse(i_control);
+    bp.parse(tool_vector_actual);
+    bp.parse(tcp_speed_actual);
+    bp.parse(tcp_force);
+    bp.parse(tool_vector_target);
+    bp.parse(tcp_speed_target);
+
+    parse_shared2(bp);
+
+    bp.parse(joint_modes);
+    bp.parse(safety_mode);
+    bp.consume(sizeof(double[6])); //skip undocumented
+    bp.parse(tool_accelerometer_values);
+    bp.consume(sizeof(double[6])); //skip undocumented
+    bp.parse(speed_scaling);
+    bp.parse(linear_momentum_norm);
+    bp.consume(sizeof(double)); //skip undocumented
+    bp.consume(sizeof(double)); //skip undocumented    
+    bp.parse(v_main);
+    bp.parse(v_robot);
+    bp.parse(i_robot);
+    bp.parse(v_actual);
+
+    return true;
+}
+
+bool RTState_V3_2__3::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V3_2__3>())
+        return false;
+
+    RTState_V3_0__1::parse_with(bp);
+
+    bp.parse(digital_outputs);
+    bp.parse(program_state);
+
+    return true;
+}
+
+bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
 }

src/ur/state.cpp

@@ -2,6 +2,10 @@
 #include "ur_modern_driver/ur/state.h"
 
 
+
+//StatePacket::~StatePacket() { }
+
+/*
 bool RobotState::parse_with(BinParser &bp) {
     //continue as long as there are bytes to read
     while(!bp.empty()) {
@@ -109,4 +113,5 @@ bool RobotState_V3_0__1::parse_package(BinParser &bp) {
 
 bool RobotState_V3_2::parse_package(BinParser &bp) {
     return parse_advanced(bp, *this);
-}
+}
+*/