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

Added clang formatting

Browse Source
This commit is contained in:
Simon Rasmussen
2017-02-16 02:03:40 +01:00
parent e00cfac0ee
commit a78d3eadf3
46 changed files with 4476 additions and 4212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
include/ur_modern_driver/ur/consumer.h
View File

@@ -1,46 +1,47 @@
#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/state.h"
class URRTPacketConsumer : public IConsumer<RTPacket> {
public:
virtual bool consume(unique_ptr<RTPacket> packet) {
virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*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;
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(unique_ptr<StatePacket> packet) {
virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*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;
};
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(unique_ptr<MessagePacket> packet) {
virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(VersionMessage &message) = 0;
virtual bool consume(VersionMessage& message) = 0;
};

72
include/ur_modern_driver/ur/factory.h
View File

@@ -1,14 +1,13 @@
#pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/state_parser.h"
#include "ur_modern_driver/ur/rt_parser.h"
#include "ur_modern_driver/ur/messages_parser.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/rt_parser.h"
#include "ur_modern_driver/ur/state_parser.h"
#include "ur_modern_driver/ur/stream.h"
#include <cstdlib>
class URFactory : private URMessagePacketConsumer {
private:
@@ -18,7 +17,8 @@ private:
uint8_t _major_version;
uint8_t _minor_version;
bool consume(VersionMessage &vm) {
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);
@@ -26,31 +26,33 @@ private:
_major_version = vm.major_version;
_minor_version = vm.minor_version;
return true;
}
void setup_consumer() { }
void teardown_consumer() { }
void stop_consumer() { }
void setup_consumer() {}
void teardown_consumer() {}
void stop_consumer() {}
public:
URFactory(std::string &host) : _stream(host, 30001) {
URFactory(std::string& host)
: _stream(host, 30001)
{
URProducer<MessagePacket> p(_stream, _parser);
std::vector<unique_ptr<MessagePacket>> results;
std::vector<unique_ptr<MessagePacket> > results;
p.setup_producer();
if(!p.try_get(results) || results.size() == 0) {
LOG_FATAL("No version message received, init failed!");
if (!p.try_get(results) || results.size() == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
for(auto const& p : results) {
for (auto const& p : results) {
p->consume_with(*this);
}
if(_major_version == 0 && _minor_version == 0) {
if (_major_version == 0 && _minor_version == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
@@ -58,28 +60,30 @@ public:
p.teardown_producer();
}
std::unique_ptr<URParser<StatePacket>> get_state_parser() {
if(_major_version == 1) {
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
std::unique_ptr<URParser<StatePacket> > get_state_parser()
{
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);
if (_minor_version < 3)
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_0__1);
else
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_2);
}
}
std::unique_ptr<URParser<RTPacket>> get_rt_parser() {
if(_major_version == 1) {
if(_minor_version < 8)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
std::unique_ptr<URParser<RTPacket> > get_rt_parser()
{
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);
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);
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);
}
}
};

35
include/ur_modern_driver/ur/master_board.h
View File

@@ -1,23 +1,22 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedMasterBoardData {
public:
virtual bool parse_with(BinParser &bp);
int8_t analog_input_range0;
virtual bool parse_with(BinParser& bp);
int8_t analog_input_range0;
int8_t analog_input_range1;
double analog_input0;
double analog_input0;
double analog_input1;
int8_t analog_output_domain0;
int8_t analog_output_domain0;
int8_t analog_output_domain1;
double analog_output0;
double analog_output0;
double analog_output1;
float master_board_temperature;
float robot_voltage_48V;
@@ -30,7 +29,7 @@ public:
int32_t euromap_input_bits;
int32_t euromap_output_bits;
static const size_t SIZE = sizeof(double) * 4
static const size_t SIZE = sizeof(double) * 4
+ sizeof(int8_t) * 4
+ sizeof(float) * 4
+ sizeof(uint8_t);
@@ -40,8 +39,8 @@ public:
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
int16_t digital_input_bits;
int16_t digital_output_bits;
@@ -53,7 +52,6 @@ public:
int16_t euromap_voltage;
int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int16_t) * 2
+ sizeof(uint8_t) * 2;
@@ -64,8 +62,8 @@ public:
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
int32_t digital_input_bits;
int32_t digital_output_bits;
@@ -77,7 +75,6 @@ public:
float euromap_voltage;
float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int32_t) * 2
+ sizeof(uint8_t) * 2
@@ -89,8 +86,8 @@ public:
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input;

44
include/ur_modern_driver/ur/messages.h
View File

@@ -1,30 +1,33 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.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
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 parse_with(BinParser &bp) = 0;
virtual bool consume_with(URMessagePacketConsumer &consumer) = 0;
MessagePacket(uint64_t timestamp, uint8_t source)
: timestamp(timestamp)
, source(source)
{
}
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
uint64_t timestamp;
uint8_t source;
@@ -32,10 +35,13 @@ public:
class VersionMessage : public MessagePacket {
public:
VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { }
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URMessagePacketConsumer &consumer);
VersionMessage(uint64_t timestamp, uint8_t source)
: MessagePacket(timestamp, source)
{
}
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URMessagePacketConsumer& consumer);
std::string project_name;
uint8_t major_version;

29
include/ur_modern_driver/ur/messages_parser.h
View File

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

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

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

32
include/ur_modern_driver/ur/producer.h
View File

@@ -1,30 +1,36 @@
#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/stream.h"
template <typename T>
class URProducer : public IProducer<T> {
private:
URStream &_stream;
URParser<T> &_parser;
URStream& _stream;
URParser<T>& _parser;
public:
URProducer(URStream &stream, URParser<T> &parser)
: _stream(stream),
_parser(parser) { }
void setup_producer() {
URProducer(URStream& stream, URParser<T>& parser)
: _stream(stream)
, _parser(parser)
{
}
void setup_producer()
{
_stream.connect();
}
void teardown_producer() {
void teardown_producer()
{
_stream.disconnect();
}
void stop_producer() {
void stop_producer()
{
_stream.disconnect();
}
bool try_get(std::vector<unique_ptr<T>> &products) {
bool try_get(std::vector<unique_ptr<T> >& products)
{
//4KB should be enough to hold any packet received from UR
uint8_t buf[4096];
@@ -33,7 +39,7 @@ public:
//LOG_DEBUG("Read %d bytes from stream", len);
if(len < 1) {
if (len < 1) {
LOG_WARN("Read nothing from stream");
return false;
}

73
include/ur_modern_driver/ur/robot_mode.h
View File

@@ -1,14 +1,14 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedRobotModeData {
public:
virtual bool parse_with(BinParser &bp);
virtual bool parse_with(BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
@@ -23,31 +23,30 @@ public:
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
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 parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
bool security_stopped;
robot_mode_V1_X robot_mode;
double speed_fraction;
static const size_t SIZE = SharedRobotModeData::SIZE
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V1_X)
+ sizeof(robot_mode_V1_X)
+ sizeof(double);
static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
@@ -55,14 +54,14 @@ public:
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
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 {
@@ -74,9 +73,8 @@ enum class robot_control_mode_V3_X : uint8_t {
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
bool protective_stopped;
@@ -85,10 +83,10 @@ public:
double target_speed_fraction;
double speed_scaling;
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V3_X)
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);
@@ -98,14 +96,13 @@ public:
class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(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");
static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
};

18
include/ur_modern_driver/ur/rt_parser.h
View File

@@ -1,31 +1,31 @@
#pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/rt_state.h"
#include <vector>
template <typename T>
class URRTStateParser : public URParser<RTPacket> {
public:
bool parse(BinParser &bp, std::vector<std::unique_ptr<RTPacket>> &results) {
bool parse(BinParser& bp, std::vector<std::unique_ptr<RTPacket> >& results)
{
int32_t packet_size = bp.peek<int32_t>();
if(!bp.check_size(packet_size)) {
if (!bp.check_size(packet_size)) {
LOG_ERROR("Buffer len shorter than expected packet length");
return false;
return false;
}
bp.parse(packet_size); //consumes the peeked data
std::unique_ptr<RTPacket> packet(new T);
if(!packet->parse_with(bp))
if (!packet->parse_with(bp))
return false;
results.push_back(std::move(packet));
return true;
}
};

42
include/ur_modern_driver/ur/rt_state.h
View File

@@ -1,23 +1,23 @@
#pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/types.h"
#include <cstddef>
#include <inttypes.h>
class URRTPacketConsumer;
class RTPacket {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool consume_with(URRTPacketConsumer &consumer) = 0;
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URRTPacketConsumer& consumer) = 0;
};
class RTShared {
protected:
bool parse_shared1(BinParser &bp);
bool parse_shared2(BinParser &bp);
bool parse_shared1(BinParser& bp);
bool parse_shared2(BinParser& bp);
public:
double time;
@@ -43,18 +43,16 @@ public:
double controller_time;
double robot_mode;
static const size_t SIZE = sizeof(double) * 3
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 parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
double3_t tool_accelerometer_values;
@@ -66,8 +64,8 @@ public:
class RTState_V1_8 : public RTState_V1_6__7 {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
double joint_modes[6];
@@ -79,14 +77,13 @@ public:
class RTState_V3_0__1 : public RTShared, public RTPacket {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
double i_control[6];
cartesian_coord_t tool_vector_target;
cartesian_coord_t tcp_speed_target;
double joint_modes[6];
double safety_mode;
double3_t tool_accelerometer_values;
@@ -97,8 +94,7 @@ public:
double i_robot;
double v_actual[6];
static const size_t SIZE = RTShared::SIZE
static const size_t SIZE = RTShared::SIZE
+ sizeof(double[6]) * 3
+ sizeof(double3_t)
+ sizeof(cartesian_coord_t) * 2
@@ -109,15 +105,15 @@ public:
class RTState_V3_2__3 : public RTState_V3_0__1 {
public:
bool parse_with(BinParser &bp);
virtual bool consume_with(URRTPacketConsumer &consumer);
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
uint64_t digital_outputs;
double program_state;
static const size_t SIZE = RTState_V3_0__1::SIZE
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!");
static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
};

34
include/ur_modern_driver/ur/state.h
View File

@@ -1,27 +1,27 @@
#pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.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
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,
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
};
@@ -29,6 +29,6 @@ class URStatePacketConsumer;
class StatePacket {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool consume_with(URStatePacketConsumer &consumer) = 0;
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
};

57
include/ur_modern_driver/ur/state_parser.h
View File

@@ -1,73 +1,74 @@
#pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
#include <vector>
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;
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) {
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) {
if (type != message_type::ROBOT_STATE) {
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
while(!bp.empty()) {
if(!bp.check_size(sizeof(uint32_t))){
while (!bp.empty()) {
if (!bp.check_size(sizeof(uint32_t))) {
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if(!bp.check_size(static_cast<size_t>(sub_size))) {
if (!bp.check_size(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
//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) {
if (packet == nullptr) {
sbp.consume();
LOG_INFO("Skipping sub-packet of type %d", type);
continue;
}
if(!packet->parse_with(sbp)) {
if (!packet->parse_with(sbp)) {
LOG_ERROR("Sub-package parsing of type %d failed!", type);
return false;
}
results.push_back(std::move(packet));
if(!sbp.empty()) {
if (!sbp.empty()) {
LOG_ERROR("Sub-package was not parsed completely!");
return false;
}
@@ -77,6 +78,6 @@ public:
}
};
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_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;

29
include/ur_modern_driver/ur/stream.h
View File

@@ -1,11 +1,11 @@
#pragma once
#include <string>
#include <atomic>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
/// Encapsulates a TCP socket
/// Encapsulates a TCP socket
class URStream {
private:
int _socket_fd = -1;
@@ -16,19 +16,22 @@ private:
std::atomic<bool> _stopping;
public:
URStream(std::string &host, int port)
: _host(host),
_port(port),
_initialized(false),
_stopping(false) {}
~URStream() {
URStream(std::string& host, int port)
: _host(host)
, _port(port)
, _initialized(false)
, _stopping(false)
{
}
~URStream()
{
disconnect();
}
bool connect();
void disconnect();
ssize_t send(uint8_t *buf, size_t buf_len);
ssize_t receive(uint8_t *buf, size_t buf_len);
ssize_t send(uint8_t* buf, size_t buf_len);
ssize_t receive(uint8_t* buf, size_t buf_len);
};