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

Adopted roscpp code style and naming convention

Browse Source
This commit is contained in:
Simon Rasmussen
2017-03-01 13:59:48 +01:00
parent e4bc40fc09
commit 474f469e97
44 changed files with 5097 additions and 4891 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,41 +7,44 @@
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/state.h"
class URRTPacketConsumer : public IConsumer<RTPacket> {
class URRTPacketConsumer : public IConsumer<RTPacket>
{
public:
virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consumeWith(*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> {
class URStatePacketConsumer : public IConsumer<StatePacket>
{
public:
virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consumeWith(*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(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(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> {
class URMessagePacketConsumer : public IConsumer<MessagePacket>
{
public:
virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consumeWith(*this);
}
virtual bool consume(VersionMessage& message) = 0;
virtual bool consume(VersionMessage& message) = 0;
};

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

@@ -1,5 +1,6 @@
#pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/messages_parser.h"
#include "ur_modern_driver/ur/parser.h"
@@ -7,83 +8,97 @@
#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 {
class URFactory : private URMessagePacketConsumer
{
private:
URStream _stream;
URMessageParser _parser;
URStream stream_;
URMessageParser parser_;
uint8_t _major_version;
uint8_t _minor_version;
uint8_t major_version_;
uint8_t minor_version_;
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);
LOG_INFO("build date: %s", vm.build_date.c_str());
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);
LOG_INFO("build date: %s", vm.build_date.c_str());
_major_version = vm.major_version;
_minor_version = vm.minor_version;
major_version_ = vm.major_version;
minor_version_ = vm.minor_version;
return true;
}
return true;
}
void setup_consumer() {}
void teardown_consumer() {}
void stop_consumer() {}
void setupConsumer()
{
}
void teardownConsumer()
{
}
void stopConsumer()
{
}
public:
URFactory(std::string& host)
: _stream(host, 30001)
URFactory(std::string& host) : stream_(host, 30001)
{
URProducer<MessagePacket> prod(stream_, parser_);
std::vector<unique_ptr<MessagePacket>> results;
prod.setupProducer();
if (!prod.tryGet(results) || results.size() == 0)
{
URProducer<MessagePacket> p(_stream, _parser);
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!");
std::exit(EXIT_FAILURE);
}
for (auto const& p : results) {
p->consume_with(*this);
}
if (_major_version == 0 && _minor_version == 0) {
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
p.teardown_producer();
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
std::unique_ptr<URParser<StatePacket> > get_state_parser()
for (auto const& p : results)
{
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);
else
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_2);
}
p->consumeWith(*this);
}
std::unique_ptr<URParser<RTPacket> > get_rt_parser()
if (major_version_ == 0 && minor_version_ == 0)
{
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);
} 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);
}
LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE);
}
prod.teardownProducer();
}
std::unique_ptr<URParser<StatePacket>> getStateParser()
{
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);
else
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
}
}
std::unique_ptr<URParser<RTPacket>> getRTParser()
{
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);
}
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);
}
}
};

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

@@ -1,97 +1,91 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#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>
class SharedMasterBoardData {
class SharedMasterBoardData
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool parseWith(BinParser& bp);
int8_t analog_input_range0;
int8_t analog_input_range1;
double analog_input0;
double analog_input1;
int8_t analog_output_domain0;
int8_t analog_output_domain1;
double analog_output0;
double analog_output1;
float master_board_temperature;
float robot_voltage_48V;
float robot_current;
float master_IO_current;
int8_t analog_input_range0;
int8_t analog_input_range1;
double analog_input0;
double analog_input1;
int8_t analog_output_domain0;
int8_t analog_output_domain1;
double analog_output0;
double analog_output1;
float master_board_temperature;
float robot_voltage_48V;
float robot_current;
float master_IO_current;
bool euromap67_interface_installed;
bool euromap67_interface_installed;
//euromap fields are dynamic so don't include in SIZE
int32_t euromap_input_bits;
int32_t euromap_output_bits;
// euromap fields are dynamic so don't include in SIZE
int32_t euromap_input_bits;
int32_t euromap_output_bits;
static const size_t SIZE = sizeof(double) * 4
+ sizeof(int8_t) * 4
+ sizeof(float) * 4
+ sizeof(uint8_t);
static const size_t SIZE = sizeof(double) * 4 + sizeof(int8_t) * 4 + sizeof(float) * 4 + sizeof(uint8_t);
static const size_t EURO_SIZE = sizeof(int32_t) * 2;
static const size_t EURO_SIZE = sizeof(int32_t) * 2;
};
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
int16_t digital_input_bits;
int16_t digital_output_bits;
int16_t digital_input_bits;
int16_t digital_output_bits;
uint8_t master_safety_state;
bool master_on_off_state;
uint8_t master_safety_state;
bool master_on_off_state;
//euromap fields are dynamic so don't include in SIZE
int16_t euromap_voltage;
int16_t euromap_current;
// euromap fields are dynamic so don't include in SIZE
int16_t euromap_voltage;
int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int16_t) * 2
+ sizeof(uint8_t) * 2;
static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int16_t) * 2 + sizeof(uint8_t) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+ sizeof(int16_t) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(int16_t) * 2;
};
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
int32_t digital_input_bits;
int32_t digital_output_bits;
int32_t digital_input_bits;
int32_t digital_output_bits;
uint8_t safety_mode;
bool in_reduced_mode;
uint8_t safety_mode;
bool in_reduced_mode;
//euromap fields are dynamic so don't include in SIZE
float euromap_voltage;
float euromap_current;
// euromap fields are dynamic so don't include in SIZE
float euromap_voltage;
float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int32_t) * 2
+ sizeof(uint8_t) * 2
+ sizeof(uint32_t); //UR internal field we skip
static const size_t SIZE = SharedMasterBoardData::SIZE + sizeof(int32_t) * 2 + sizeof(uint8_t) * 2 +
sizeof(uint32_t); // UR internal field we skip
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+ sizeof(float) * 2;
static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE + sizeof(float) * 2;
};
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input;
uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input;
static const size_t SIZE = MasterBoardData_V3_0__1::SIZE
+ sizeof(uint8_t) * 2;
static const size_t SIZE = MasterBoardData_V3_0__1::SIZE + sizeof(uint8_t) * 2;
};

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

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

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

@@ -1,49 +1,53 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/parser.h"
#include <vector>
class URMessageParser : public URParser<MessagePacket> {
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)
{
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
if (type != message_type::ROBOT_MESSAGE) {
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
uint64_t timestamp;
uint8_t source;
robot_message_type message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
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;
}
default:
return false;
}
results.push_back(std::move(result));
return parsed;
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
uint64_t timestamp;
uint8_t source;
robot_message_type message_type;
bp.parse(timestamp);
bp.parse(source);
bp.parse(message_type);
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->parseWith(bp);
result.reset(vm);
break;
}
default:
return false;
}
results.push_back(std::move(result));
return parsed;
}
};

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

@@ -1,10 +1,11 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include <vector>
template <typename T>
class URParser {
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;
};

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

@@ -4,41 +4,52 @@
#include "ur_modern_driver/ur/stream.h"
template <typename T>
class URProducer : public IProducer<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)
URProducer(URStream& stream, URParser<T>& parser) : stream_(stream), parser_(parser)
{
}
void setupProducer()
{
stream_.connect();
}
void teardownProducer()
{
stream_.disconnect();
}
void stopProducer()
{
stream_.disconnect();
}
bool tryGet(std::vector<unique_ptr<T>>& products)
{
// 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 == 0)
{
LOG_WARN("Read nothing from stream");
return false;
}
else if (len < 0)
{
LOG_WARN("Stream closed");
return false;
}
void setup_producer() { _stream.connect(); }
void teardown_producer() { _stream.disconnect(); }
void stop_producer() { _stream.disconnect(); }
bool try_get(std::vector<unique_ptr<T> >& products)
{
//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 == 0) {
LOG_WARN("Read nothing from stream");
return false;
} else if (len < 0) {
LOG_WARN("Stream closed");
return false;
}
BinParser bp(buf, static_cast<size_t>(len));
return _parser.parse(bp, products);
}
BinParser bp(buf, static_cast<size_t>(len));
return parser_.parse(bp, products);
}
};

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

@@ -1,108 +1,107 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#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>
class SharedRobotModeData {
class SharedRobotModeData
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool parseWith(BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
bool real_robot_enabled;
bool robot_power_on;
bool emergency_stopped;
bool program_running;
bool program_paused;
uint64_t timestamp;
bool physical_robot_connected;
bool real_robot_enabled;
bool robot_power_on;
bool emergency_stopped;
bool program_running;
bool program_paused;
static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
};
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
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
};
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket {
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
bool security_stopped;
robot_mode_V1_X robot_mode;
double speed_fraction;
bool security_stopped;
robot_mode_V1_X robot_mode;
double speed_fraction;
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V1_X)
+ sizeof(double);
static const size_t SIZE = SharedRobotModeData::SIZE + sizeof(uint8_t) + sizeof(robot_mode_V1_X) + sizeof(double);
static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
};
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
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
};
enum class robot_control_mode_V3_X : uint8_t {
POSITION = 0,
TEACH = 1,
FORCE = 2,
TORQUE = 3
enum class robot_control_mode_V3_X : uint8_t
{
POSITION = 0,
TEACH = 1,
FORCE = 2,
TORQUE = 3
};
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
bool protective_stopped;
bool protective_stopped;
robot_mode_V3_X robot_mode;
robot_control_mode_V3_X control_mode;
robot_mode_V3_X robot_mode;
robot_control_mode_V3_X control_mode;
double target_speed_fraction;
double speed_scaling;
double target_speed_fraction;
double speed_scaling;
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);
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);
static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
};
class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
class RobotModeData_V3_2 : public RobotModeData_V3_0__1
{
public:
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
double target_speed_fraction_limit;
double target_speed_fraction_limit;
static const size_t SIZE = RobotModeData_V3_0__1::SIZE
+ sizeof(double);
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");
};

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

@@ -1,33 +1,35 @@
#pragma once
#include <vector>
#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> {
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.checkSize(packet_size))
{
int32_t packet_size = bp.peek<int32_t>();
if (!bp.check_size(packet_size)) {
LOG_ERROR("Buffer len shorter than expected packet length");
return false;
}
bp.parse(packet_size); //consumes the peeked data
std::unique_ptr<RTPacket> packet(new T);
if (!packet->parse_with(bp))
return false;
results.push_back(std::move(packet));
return true;
LOG_ERROR("Buffer len shorter than expected packet length");
return false;
}
bp.parse(packet_size); // consumes the peeked data
std::unique_ptr<RTPacket> packet(new T);
if (!packet->parseWith(bp))
return false;
results.push_back(std::move(packet));
return true;
}
};
typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;

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

@@ -1,122 +1,119 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#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 {
class RTPacket
{
public:
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URRTPacketConsumer& consumer) = 0;
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URRTPacketConsumer& consumer) = 0;
};
class RTShared {
class RTShared
{
protected:
void parse_shared1(BinParser& bp);
void parse_shared2(BinParser& bp);
void parse_shared1(BinParser& bp);
void parse_shared2(BinParser& bp);
public:
double time;
double q_target[6];
double qd_target[6];
double qdd_target[6];
double i_target[6];
double m_target[6];
double q_actual[6];
double qd_actual[6];
double i_actual[6];
double time;
double q_target[6];
double qd_target[6];
double qdd_target[6];
double i_target[6];
double m_target[6];
double q_actual[6];
double qd_actual[6];
double i_actual[6];
//gap here depending on version
// gap here depending on version
double tcp_force[6];
double tcp_force[6];
//does not contain "_actual" postfix in V1_X but
//they're the same fields so share anyway
cartesian_coord_t tool_vector_actual;
cartesian_coord_t tcp_speed_actual;
// does not contain "_actual" postfix in V1_X but
// they're the same fields so share anyway
cartesian_coord_t tool_vector_actual;
cartesian_coord_t tcp_speed_actual;
//gap here depending on version
// gap here depending on version
uint64_t digital_inputs;
double motor_temperatures[6];
double controller_time;
double robot_mode;
uint64_t digital_inputs;
double motor_temperatures[6];
double controller_time;
double robot_mode;
static const size_t SIZE = sizeof(double) * 3
+ sizeof(double[6]) * 10
+ sizeof(cartesian_coord_t) * 2
+ sizeof(uint64_t);
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 {
class RTState_V1_6__7 : public RTShared, public RTPacket
{
public:
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
double3_t tool_accelerometer_values;
double3_t tool_accelerometer_values;
static const size_t SIZE = RTShared::SIZE
+ sizeof(double3_t);
static const size_t SIZE = RTShared::SIZE + sizeof(double3_t);
static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
};
class RTState_V1_8 : public RTState_V1_6__7 {
class RTState_V1_8 : public RTState_V1_6__7
{
public:
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
double joint_modes[6];
double joint_modes[6];
static const size_t SIZE = RTState_V1_6__7::SIZE
+ sizeof(double[6]);
static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]);
static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
};
class RTState_V3_0__1 : public RTShared, public RTPacket {
class RTState_V3_0__1 : public RTShared, public RTPacket
{
public:
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
double i_control[6];
cartesian_coord_t tool_vector_target;
cartesian_coord_t tcp_speed_target;
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;
double speed_scaling;
double linear_momentum_norm;
double v_main;
double v_robot;
double i_robot;
double v_actual[6];
double joint_modes[6];
double safety_mode;
double3_t tool_accelerometer_values;
double speed_scaling;
double linear_momentum_norm;
double v_main;
double v_robot;
double i_robot;
double v_actual[6];
static const size_t SIZE = RTShared::SIZE
+ sizeof(double[6]) * 3
+ sizeof(double3_t)
+ sizeof(cartesian_coord_t) * 2
+ sizeof(double) * 6;
static const size_t SIZE =
RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6;
static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
};
class RTState_V3_2__3 : public RTState_V3_0__1 {
class RTState_V3_2__3 : public RTState_V3_0__1
{
public:
bool parse_with(BinParser& bp);
virtual bool consume_with(URRTPacketConsumer& consumer);
bool parseWith(BinParser& bp);
virtual bool consumeWith(URRTPacketConsumer& consumer);
uint64_t digital_outputs;
double program_state;
uint64_t digital_outputs;
double program_state;
static const size_t SIZE = RTState_V3_0__1::SIZE
+ sizeof(uint64_t)
+ sizeof(double);
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!");
};

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

@@ -1,34 +1,37 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#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
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
};
enum class message_type : uint8_t {
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
enum class message_type : uint8_t
{
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
};
class URStatePacketConsumer;
class StatePacket {
class StatePacket
{
public:
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
};

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

@@ -1,4 +1,5 @@
#pragma once
#include <vector>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
@@ -6,76 +7,84 @@
#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> {
class URStateParser : public URParser<StatePacket>
{
private:
StatePacket* from_type(package_type type)
StatePacket* from_type(package_type type)
{
switch (type)
{
switch (type) {
case package_type::ROBOT_MODE_DATA:
return new RMD;
case package_type::MASTERBOARD_DATA:
return new MBD;
default:
return nullptr;
}
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)
{
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
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))) {
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
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
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) {
sbp.consume();
LOG_INFO("Skipping sub-packet of type %d", type);
continue;
}
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()) {
LOG_ERROR("Sub-package was not parsed completely!");
return false;
}
}
return true;
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
while (!bp.empty())
{
if (!bp.checkSize(sizeof(uint32_t)))
{
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if (!bp.checkSize(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
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)
{
sbp.consume();
LOG_INFO("Skipping sub-packet of type %d", type);
continue;
}
if (!packet->parseWith(sbp))
{
LOG_ERROR("Sub-package parsing of type %d failed!", type);
return false;
}
results.push_back(std::move(packet));
if (!sbp.empty())
{
LOG_ERROR("Sub-package was not parsed completely!");
return false;
}
}
return true;
}
};
typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;

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

@@ -1,37 +1,34 @@
#pragma once
#include <atomic>
#include <netdb.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <string>
/// Encapsulates a TCP socket
class URStream {
class URStream
{
private:
int _socket_fd = -1;
std::string _host;
int _port;
int socket_fd_ = -1;
std::string host_;
int port_;
std::atomic<bool> _initialized;
std::atomic<bool> _stopping;
std::atomic<bool> initialized_;
std::atomic<bool> stopping_;
public:
URStream(std::string& host, int port)
: _host(host)
, _port(port)
, _initialized(false)
, _stopping(false)
{
}
URStream(std::string& host, int port) : host_(host), port_(port), initialized_(false), stopping_(false)
{
}
~URStream()
{
disconnect();
}
~URStream()
{
disconnect();
}
bool connect();
void 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);
};