Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Completed parsing of UR state, messages and RT

Browse Source
This commit is contained in:
Simon Rasmussen
2017-02-16 01:52:22 +01:00
parent d0fa801cad
commit c282c961f7
19 changed files with 599 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
include/ur_modern_driver/ur/parser.h
View File

@@ -1,88 +1,10 @@
#pragma once
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/parser.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/messages.h"
#include <vector>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h"
template <typename T>
class URStateParser : public Parser {
std::unique_ptr<Packet> parse(BinParser &bp) {
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
if(type != message_type::ROBOT_STATE) {
LOG_ERROR("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return std::unique_ptr<Packet>(nullptr);
}
std::unique_ptr<Packet> obj(new T);
if(obj->parse_with(bp))
return obj;
return std::unique_ptr<Packet>(nullptr);
}
class URParser {
public:
virtual bool parse(BinParser &bp, std::vector<std::unique_ptr<T>> &results) = 0;
};
template <typename T>
class URRTStateParser : public Parser {
std::unique_ptr<Packet> parse(BinParser &bp) {
int32_t packet_size = bp.peek<int32_t>();
if(!bp.check_size(packet_size)) {
LOG_ERROR("Buffer len shorter than expected packet length");
return std::unique_ptr<Packet>(nullptr);
}
bp.parse(packet_size); //consumes the peeked data
std::unique_ptr<Packet> obj(new T);
if(obj->parse_with(bp))
return obj;
return std::unique_ptr<Packet>(nullptr);
}
};
class URMessageParser : public Parser {
std::unique_ptr<Packet> parse(BinParser &bp) {
int32_t packet_size = bp.peek<int32_t>();
message_type type;
if(!bp.check_size(packet_size)) {
LOG_ERROR("Buffer len shorter than expected packet length");
return std::unique_ptr<Packet>(nullptr);
}
bp.parse(packet_size); //consumes the peeked data
bp.parse(type);
if(type != message_type::ROBOT_MESSAGE) {
LOG_ERROR("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return std::unique_ptr<Packet>(nullptr);
}
uint64_t timestamp;
uint8_t source;
robot_message_type message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
std::unique_ptr<Packet> obj(nullptr);
switch(message_type) {
case robot_message_type::ROBOT_MESSAGE_VERSION:
VersionMessage *vm = new VersionMessage();
if(vm->parse_with(bp))
obj.reset(vm);
break;
}
return obj;
}
};