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

Refactoring, improvements and fixes

Browse Source
This commit is contained in:
Simon Rasmussen
2017-02-09 12:16:51 +01:00
parent e94bbb5536
commit 43974dcbf2
10 changed files with 258 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,23 +2,19 @@
#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"
template <typename T>
class URStateParser : public Parser {
std::unique_ptr<Packet> parse(BinParser &bp) {
int32_t packet_size = bp.peek<int32_t>();
std::unique_ptr<Packet> parse(BinParser &bp) {
int32_t packet_size;
message_type type;
if(!bp.check_size(packet_size)) {
LOG_ERROR("Buffer len shorter than expected packet length\n");
return std::unique_ptr<Packet>(nullptr);
}
bp.parse(packet_size); //consumes the peeked data
bp.parse(packet_size);
bp.parse(type);
if(type != message_type::ROBOT_STATE) {
LOG_ERROR("Invalid message type recieved: %u\n", static_cast<uint8_t>(type));
LOG_ERROR("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return std::unique_ptr<Packet>(nullptr);
}
@@ -33,11 +29,11 @@ class URStateParser : public Parser {
template <typename T>
class URRTStateParser : public Parser {
std::unique_ptr<Packet> parse(BinParser &bp) {
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\n");
LOG_ERROR("Buffer len shorter than expected packet length");
return std::unique_ptr<Packet>(nullptr);
}
@@ -49,4 +45,44 @@ class URRTStateParser : public Parser {
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;
}
};