diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h new file mode 100644 index 0000000..d7a1222 --- /dev/null +++ b/include/ur_modern_driver/parser.h @@ -0,0 +1,8 @@ +#pragma once +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/packet.h" + +class Parser { +public: + virtual std::unique_ptr parse(BinParser &bp) = 0; +}; diff --git a/include/ur_modern_driver/ur/parser.h b/include/ur_modern_driver/ur/parser.h new file mode 100644 index 0000000..573b3d4 --- /dev/null +++ b/include/ur_modern_driver/ur/parser.h @@ -0,0 +1,52 @@ +#pragma once +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/parser.h" +#include "ur_modern_driver/ur/state.h" + +template +class URStateParser : public Parser { + std::unique_ptr parse(BinParser &bp) { + int32_t packet_size = bp.peek(); + message_type type; + + if(!bp.check_size(packet_size)) { + LOG_ERROR("Buffer len shorter than expected packet length\n"); + return std::unique_ptr(nullptr); + } + + bp.parse(packet_size); //consumes the peeked data + bp.parse(type); + + if(type != message_type::ROBOT_STATE) { + LOG_ERROR("Invalid message type recieved: %u\n", static_cast(type)); + return std::unique_ptr(nullptr); + } + + std::unique_ptr obj(new T); + if(obj->parse_with(bp)) + return obj; + + return std::unique_ptr(nullptr); + } +}; + + +template +class URRTStateParser : public Parser { + std::unique_ptr parse(BinParser &bp) { + int32_t packet_size = bp.peek(); + + if(!bp.check_size(packet_size)) { + LOG_ERROR("Buffer len shorter than expected packet length\n"); + return std::unique_ptr(nullptr); + } + + bp.parse(packet_size); //consumes the peeked data + + std::unique_ptr obj(new T); + if(obj->parse_with(bp)) + return obj; + + return std::unique_ptr(nullptr); + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h new file mode 100644 index 0000000..b86b519 --- /dev/null +++ b/include/ur_modern_driver/ur/producer.h @@ -0,0 +1,21 @@ +#pragma once +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/stream.h" +#include "ur_modern_driver/packet.h" +#include "ur_modern_driver/parser.h" + +class URProducer : public IProducer { +private: + URStream &_stream; + Parser &_parser; + +public: + URProducer(URStream &stream, Parser &parser) + : _stream(stream), + _parser(parser) { } + + void setup_producer(); + void teardown_producer(); + void stop_producer(); + std::unique_ptr try_get(); +}; \ No newline at end of file diff --git a/src/ur/producer.cpp b/src/ur/producer.cpp new file mode 100644 index 0000000..f1ca009 --- /dev/null +++ b/src/ur/producer.cpp @@ -0,0 +1,54 @@ +#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 URProducer::try_get() { + //4KB should be enough to hold any packet received from UR + uint8_t buf[4096]; + + ssize_t total = 0; + int32_t packet_size = 0; + + //deal with partial recieves + while(total <= sizeof(buf)) { + uint8_t *pos = buf + total; + size_t size = sizeof(buf) - total; + + //blocking call + ssize_t len = _stream.receive(pos, size); + + if(len < 1) { + LOG_DEBUG("Read nothing from stream\n"); + return std::unique_ptr(nullptr); + } + + total += len; + BinParser bp(buf, static_cast(total)); + + if(packet_size == 0) { + packet_size = bp.peek(); + //TODO: check other wrong packet sizes? + if(packet_size > sizeof(buf)) { + LOG_ERROR("A packet with 'len' larger than buffer was received, discarding...\n"); + return std::unique_ptr(nullptr); + } + } + + if(total < packet_size){ + LOG_DEBUG("Partial packet recieved\n"); + continue; + } + return std::move(_parser.parse(bp)); + } +} \ No newline at end of file