mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
Adopted roscpp code style and naming convention
This commit is contained in:
Simon Rasmussen
@@ -1,178 +1,175 @@
|
||||
#pragma once
|
||||
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/types.h"
|
||||
#include <assert.h>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <endian.h>
|
||||
#include <inttypes.h>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/types.h"
|
||||
|
||||
class BinParser {
|
||||
class BinParser
|
||||
{
|
||||
private:
|
||||
uint8_t *_buf_pos, *_buf_end;
|
||||
BinParser& _parent;
|
||||
uint8_t *buf_pos_, *buf_end_;
|
||||
BinParser& parent_;
|
||||
|
||||
public:
|
||||
BinParser(uint8_t* buffer, size_t buf_len)
|
||||
: _buf_pos(buffer)
|
||||
, _buf_end(buffer + buf_len)
|
||||
, _parent(*this)
|
||||
{
|
||||
assert(_buf_pos <= _buf_end);
|
||||
}
|
||||
BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
|
||||
{
|
||||
assert(buf_pos_ <= buf_end_);
|
||||
}
|
||||
|
||||
BinParser(BinParser& parent, size_t sub_len)
|
||||
: _buf_pos(parent._buf_pos)
|
||||
, _buf_end(parent._buf_pos + sub_len)
|
||||
, _parent(parent)
|
||||
{
|
||||
assert(_buf_pos <= _buf_end);
|
||||
}
|
||||
BinParser(BinParser& parent, size_t sub_len)
|
||||
: buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent)
|
||||
{
|
||||
assert(buf_pos_ <= buf_end_);
|
||||
}
|
||||
|
||||
~BinParser()
|
||||
{
|
||||
_parent._buf_pos = _buf_pos;
|
||||
}
|
||||
~BinParser()
|
||||
{
|
||||
parent_.buf_pos_ = buf_pos_;
|
||||
}
|
||||
|
||||
//Decode from network encoding (big endian) to host encoding
|
||||
template <typename T>
|
||||
T decode(T val)
|
||||
{
|
||||
return val;
|
||||
}
|
||||
uint16_t decode(uint16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
uint32_t decode(uint32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
uint64_t decode(uint64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
int16_t decode(int16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
int32_t decode(int32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
int64_t decode(int64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
// Decode from network encoding (big endian) to host encoding
|
||||
template <typename T>
|
||||
T decode(T val)
|
||||
{
|
||||
return val;
|
||||
}
|
||||
uint16_t decode(uint16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
uint32_t decode(uint32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
uint64_t decode(uint64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
int16_t decode(int16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
int32_t decode(int32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
int64_t decode(int64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T peek()
|
||||
{
|
||||
assert(_buf_pos <= _buf_end);
|
||||
T val;
|
||||
std::memcpy(&val, _buf_pos, sizeof(T));
|
||||
return decode(val);
|
||||
}
|
||||
template <typename T>
|
||||
T peek()
|
||||
{
|
||||
assert(buf_pos_ <= buf_end_);
|
||||
T val;
|
||||
std::memcpy(&val, buf_pos_, sizeof(T));
|
||||
return decode(val);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void parse(T& val)
|
||||
{
|
||||
val = peek<T>();
|
||||
_buf_pos += sizeof(T);
|
||||
}
|
||||
template <typename T>
|
||||
void parse(T& val)
|
||||
{
|
||||
val = peek<T>();
|
||||
buf_pos_ += sizeof(T);
|
||||
}
|
||||
|
||||
void parse(double& val)
|
||||
{
|
||||
uint64_t inner;
|
||||
parse<uint64_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(double));
|
||||
}
|
||||
void parse(float& val)
|
||||
{
|
||||
uint32_t inner;
|
||||
parse<uint32_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(float));
|
||||
}
|
||||
void parse(double& val)
|
||||
{
|
||||
uint64_t inner;
|
||||
parse<uint64_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(double));
|
||||
}
|
||||
void parse(float& val)
|
||||
{
|
||||
uint32_t inner;
|
||||
parse<uint32_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(float));
|
||||
}
|
||||
|
||||
// UR uses 1 byte for boolean values but sizeof(bool) is implementation
|
||||
// defined so we must ensure they're parsed as uint8_t on all compilers
|
||||
void parse(bool& val)
|
||||
{
|
||||
uint8_t inner;
|
||||
parse<uint8_t>(inner);
|
||||
val = inner != 0;
|
||||
}
|
||||
// UR uses 1 byte for boolean values but sizeof(bool) is implementation
|
||||
// defined so we must ensure they're parsed as uint8_t on all compilers
|
||||
void parse(bool& val)
|
||||
{
|
||||
uint8_t inner;
|
||||
parse<uint8_t>(inner);
|
||||
val = inner != 0;
|
||||
}
|
||||
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(double3_t& val)
|
||||
{
|
||||
parse(val.x);
|
||||
parse(val.y);
|
||||
parse(val.z);
|
||||
}
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(double3_t& val)
|
||||
{
|
||||
parse(val.x);
|
||||
parse(val.y);
|
||||
parse(val.z);
|
||||
}
|
||||
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(cartesian_coord_t& val)
|
||||
{
|
||||
parse(val.position);
|
||||
parse(val.rotation);
|
||||
}
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(cartesian_coord_t& val)
|
||||
{
|
||||
parse(val.position);
|
||||
parse(val.rotation);
|
||||
}
|
||||
|
||||
void parse_remainder(std::string& val)
|
||||
{
|
||||
parse(val, size_t(_buf_end - _buf_pos));
|
||||
}
|
||||
void parse_remainder(std::string& val)
|
||||
{
|
||||
parse(val, size_t(buf_end_ - buf_pos_));
|
||||
}
|
||||
|
||||
void parse(std::string& val, size_t len)
|
||||
{
|
||||
val.assign(reinterpret_cast<char*>(_buf_pos), len);
|
||||
_buf_pos += len;
|
||||
}
|
||||
void parse(std::string& val, size_t len)
|
||||
{
|
||||
val.assign(reinterpret_cast<char*>(buf_pos_), len);
|
||||
buf_pos_ += len;
|
||||
}
|
||||
|
||||
// Special string parse function that assumes uint8_t len followed by chars
|
||||
void parse(std::string& val)
|
||||
{
|
||||
uint8_t len;
|
||||
parse(len);
|
||||
parse(val, size_t(len));
|
||||
}
|
||||
// Special string parse function that assumes uint8_t len followed by chars
|
||||
void parse(std::string& val)
|
||||
{
|
||||
uint8_t len;
|
||||
parse(len);
|
||||
parse(val, size_t(len));
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void parse(T (&array)[N])
|
||||
template <typename T, size_t N>
|
||||
void parse(T (&array)[N])
|
||||
{
|
||||
for (size_t i = 0; i < N; i++)
|
||||
{
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
parse(array[i]);
|
||||
}
|
||||
parse(array[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void consume()
|
||||
{
|
||||
_buf_pos = _buf_end;
|
||||
}
|
||||
void consume(size_t bytes)
|
||||
{
|
||||
_buf_pos += bytes;
|
||||
}
|
||||
void consume()
|
||||
{
|
||||
buf_pos_ = buf_end_;
|
||||
}
|
||||
void consume(size_t bytes)
|
||||
{
|
||||
buf_pos_ += bytes;
|
||||
}
|
||||
|
||||
bool check_size(size_t bytes)
|
||||
{
|
||||
return bytes <= size_t(_buf_end - _buf_pos);
|
||||
}
|
||||
template <typename T>
|
||||
bool check_size(void)
|
||||
{
|
||||
return check_size(T::SIZE);
|
||||
}
|
||||
bool checkSize(size_t bytes)
|
||||
{
|
||||
return bytes <= size_t(buf_end_ - buf_pos_);
|
||||
}
|
||||
template <typename T>
|
||||
bool checkSize(void)
|
||||
{
|
||||
return checkSize(T::SIZE);
|
||||
}
|
||||
|
||||
bool empty()
|
||||
{
|
||||
return _buf_pos == _buf_end;
|
||||
}
|
||||
bool empty()
|
||||
{
|
||||
return buf_pos_ == buf_end_;
|
||||
}
|
||||
|
||||
void debug()
|
||||
{
|
||||
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos);
|
||||
}
|
||||
void debug()
|
||||
{
|
||||
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
#pragma once
|
||||
#include "ur_modern_driver/bin_parser.h"
|
||||
|
||||
class Packet {
|
||||
class Packet
|
||||
{
|
||||
public:
|
||||
virtual bool parse_with(BinParser& bp) = 0;
|
||||
virtual bool parseWith(BinParser& bp) = 0;
|
||||
};
|
||||
@@ -2,7 +2,8 @@
|
||||
#include "ur_modern_driver/bin_parser.h"
|
||||
#include "ur_modern_driver/packet.h"
|
||||
|
||||
class Parser {
|
||||
class Parser
|
||||
{
|
||||
public:
|
||||
virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
|
||||
virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
|
||||
};
|
||||
|
||||
@@ -1,116 +1,134 @@
|
||||
#pragma once
|
||||
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/queue/readerwriterqueue.h"
|
||||
#include <atomic>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/queue/readerwriterqueue.h"
|
||||
|
||||
using namespace moodycamel;
|
||||
using namespace std;
|
||||
|
||||
template <typename T>
|
||||
class IConsumer {
|
||||
class IConsumer
|
||||
{
|
||||
public:
|
||||
virtual void setup_consumer() {}
|
||||
virtual void teardown_consumer() {}
|
||||
virtual void stop_consumer() {}
|
||||
virtual void setupConsumer()
|
||||
{
|
||||
}
|
||||
virtual void teardownConsumer()
|
||||
{
|
||||
}
|
||||
virtual void stopConsumer()
|
||||
{
|
||||
}
|
||||
|
||||
virtual bool consume(unique_ptr<T> product) = 0;
|
||||
virtual bool consume(unique_ptr<T> product) = 0;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class IProducer {
|
||||
class IProducer
|
||||
{
|
||||
public:
|
||||
virtual void setup_producer() {}
|
||||
virtual void teardown_producer() {}
|
||||
virtual void stop_producer() {}
|
||||
virtual void setupProducer()
|
||||
{
|
||||
}
|
||||
virtual void teardownProducer()
|
||||
{
|
||||
}
|
||||
virtual void stopProducer()
|
||||
{
|
||||
}
|
||||
|
||||
virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
|
||||
virtual bool tryGet(std::vector<unique_ptr<T>>& products) = 0;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class Pipeline {
|
||||
class Pipeline
|
||||
{
|
||||
private:
|
||||
IProducer<T>& _producer;
|
||||
IConsumer<T>& _consumer;
|
||||
BlockingReaderWriterQueue<unique_ptr<T> > _queue;
|
||||
atomic<bool> _running;
|
||||
thread _pThread, _cThread;
|
||||
IProducer<T>& producer_;
|
||||
IConsumer<T>& consumer_;
|
||||
BlockingReaderWriterQueue<unique_ptr<T>> queue_;
|
||||
atomic<bool> running_;
|
||||
thread pThread_, cThread_;
|
||||
|
||||
void run_producer()
|
||||
void run_producer()
|
||||
{
|
||||
producer_.setupProducer();
|
||||
std::vector<unique_ptr<T>> products;
|
||||
while (running_)
|
||||
{
|
||||
_producer.setup_producer();
|
||||
std::vector<unique_ptr<T> > products;
|
||||
while (_running) {
|
||||
if (!_producer.try_get(products)) {
|
||||
break;
|
||||
}
|
||||
if (!producer_.tryGet(products))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
for (auto& p : products) {
|
||||
if (!_queue.try_enqueue(std::move(p))) {
|
||||
LOG_ERROR("Pipeline producer owerflowed!");
|
||||
}
|
||||
}
|
||||
|
||||
products.clear();
|
||||
for (auto& p : products)
|
||||
{
|
||||
if (!queue_.try_enqueue(std::move(p)))
|
||||
{
|
||||
LOG_ERROR("Pipeline producer owerflowed!");
|
||||
}
|
||||
_producer.teardown_producer();
|
||||
LOG_DEBUG("Pipline producer ended");
|
||||
_consumer.stop_consumer();
|
||||
}
|
||||
}
|
||||
|
||||
void run_consumer()
|
||||
products.clear();
|
||||
}
|
||||
producer_.teardownProducer();
|
||||
LOG_DEBUG("Pipline producer ended");
|
||||
consumer_.stopConsumer();
|
||||
}
|
||||
|
||||
void run_consumer()
|
||||
{
|
||||
consumer_.setupConsumer();
|
||||
unique_ptr<T> product;
|
||||
while (running_)
|
||||
{
|
||||
_consumer.setup_consumer();
|
||||
unique_ptr<T> product;
|
||||
while (_running) {
|
||||
// 16000us timeout was chosen because we should
|
||||
// roughly recieve messages at 125hz which is every
|
||||
// 8ms == 8000us and double it for some error margin
|
||||
if (!_queue.wait_dequeue_timed(product, 16000)) {
|
||||
continue;
|
||||
}
|
||||
if (!_consumer.consume(std::move(product)))
|
||||
break;
|
||||
}
|
||||
_consumer.teardown_consumer();
|
||||
LOG_DEBUG("Pipline consumer ended");
|
||||
_producer.stop_producer();
|
||||
// 16000us timeout was chosen because we should
|
||||
// roughly recieve messages at 125hz which is every
|
||||
// 8ms == 8000us and double it for some error margin
|
||||
if (!queue_.wait_dequeue_timed(product, 16000))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if (!consumer_.consume(std::move(product)))
|
||||
break;
|
||||
}
|
||||
consumer_.teardownConsumer();
|
||||
LOG_DEBUG("Pipline consumer ended");
|
||||
producer_.stopProducer();
|
||||
}
|
||||
|
||||
public:
|
||||
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
|
||||
: _producer(producer)
|
||||
, _consumer(consumer)
|
||||
, _queue{ 32 }
|
||||
, _running{ false }
|
||||
{
|
||||
}
|
||||
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
|
||||
: producer_(producer), consumer_(consumer), queue_{ 32 }, running_{ false }
|
||||
{
|
||||
}
|
||||
|
||||
void run()
|
||||
{
|
||||
if (_running)
|
||||
return;
|
||||
void run()
|
||||
{
|
||||
if (running_)
|
||||
return;
|
||||
|
||||
_running = true;
|
||||
_pThread = thread(&Pipeline::run_producer, this);
|
||||
_cThread = thread(&Pipeline::run_consumer, this);
|
||||
}
|
||||
running_ = true;
|
||||
pThread_ = thread(&Pipeline::run_producer, this);
|
||||
cThread_ = thread(&Pipeline::run_consumer, this);
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
if (!_running)
|
||||
return;
|
||||
void stop()
|
||||
{
|
||||
if (!running_)
|
||||
return;
|
||||
|
||||
LOG_DEBUG("Stopping pipeline");
|
||||
LOG_DEBUG("Stopping pipeline");
|
||||
|
||||
_consumer.stop_consumer();
|
||||
_producer.stop_producer();
|
||||
consumer_.stopConsumer();
|
||||
producer_.stopProducer();
|
||||
|
||||
_running = false;
|
||||
running_ = false;
|
||||
|
||||
_pThread.join();
|
||||
_cThread.join();
|
||||
}
|
||||
pThread_.join();
|
||||
cThread_.join();
|
||||
}
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -19,202 +19,215 @@
|
||||
#ifndef ROBOT_STATE_H_
|
||||
#define ROBOT_STATE_H_
|
||||
|
||||
#include <condition_variable>
|
||||
#include <inttypes.h>
|
||||
#include <mutex>
|
||||
#include <netinet/in.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
namespace message_types {
|
||||
enum message_type {
|
||||
ROBOT_STATE = 16,
|
||||
ROBOT_MESSAGE = 20,
|
||||
PROGRAM_STATE_MESSAGE = 25
|
||||
namespace message_types
|
||||
{
|
||||
enum message_type
|
||||
{
|
||||
ROBOT_STATE = 16,
|
||||
ROBOT_MESSAGE = 20,
|
||||
PROGRAM_STATE_MESSAGE = 25
|
||||
};
|
||||
}
|
||||
typedef message_types::message_type messageType;
|
||||
|
||||
namespace package_types {
|
||||
enum package_type {
|
||||
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
|
||||
namespace package_types
|
||||
{
|
||||
enum package_type
|
||||
{
|
||||
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
|
||||
};
|
||||
}
|
||||
typedef package_types::package_type packageType;
|
||||
|
||||
namespace robot_message_types {
|
||||
enum robot_message_type {
|
||||
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
|
||||
namespace robot_message_types
|
||||
{
|
||||
enum robot_message_type
|
||||
{
|
||||
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
|
||||
};
|
||||
}
|
||||
typedef robot_message_types::robot_message_type robotMessageType;
|
||||
|
||||
namespace robot_state_type_v18 {
|
||||
enum robot_state_type {
|
||||
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
|
||||
namespace robot_state_type_v18
|
||||
{
|
||||
enum robot_state_type
|
||||
{
|
||||
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
|
||||
};
|
||||
}
|
||||
typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
|
||||
namespace robot_state_type_v30 {
|
||||
enum robot_state_type {
|
||||
ROBOT_MODE_DISCONNECTED = 0,
|
||||
ROBOT_MODE_CONFIRM_SAFETY = 1,
|
||||
ROBOT_MODE_BOOTING = 2,
|
||||
ROBOT_MODE_POWER_OFF = 3,
|
||||
ROBOT_MODE_POWER_ON = 4,
|
||||
ROBOT_MODE_IDLE = 5,
|
||||
ROBOT_MODE_BACKDRIVE = 6,
|
||||
ROBOT_MODE_RUNNING = 7,
|
||||
ROBOT_MODE_UPDATING_FIRMWARE = 8
|
||||
namespace robot_state_type_v30
|
||||
{
|
||||
enum robot_state_type
|
||||
{
|
||||
ROBOT_MODE_DISCONNECTED = 0,
|
||||
ROBOT_MODE_CONFIRM_SAFETY = 1,
|
||||
ROBOT_MODE_BOOTING = 2,
|
||||
ROBOT_MODE_POWER_OFF = 3,
|
||||
ROBOT_MODE_POWER_ON = 4,
|
||||
ROBOT_MODE_IDLE = 5,
|
||||
ROBOT_MODE_BACKDRIVE = 6,
|
||||
ROBOT_MODE_RUNNING = 7,
|
||||
ROBOT_MODE_UPDATING_FIRMWARE = 8
|
||||
};
|
||||
}
|
||||
|
||||
typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
|
||||
|
||||
struct version_message {
|
||||
uint64_t timestamp;
|
||||
int8_t source;
|
||||
int8_t robot_message_type;
|
||||
int8_t project_name_size;
|
||||
char project_name[15];
|
||||
uint8_t major_version;
|
||||
uint8_t minor_version;
|
||||
int svn_revision;
|
||||
char build_date[25];
|
||||
struct version_message
|
||||
{
|
||||
uint64_t timestamp;
|
||||
int8_t source;
|
||||
int8_t robot_message_type;
|
||||
int8_t project_name_size;
|
||||
char project_name[15];
|
||||
uint8_t major_version;
|
||||
uint8_t minor_version;
|
||||
int svn_revision;
|
||||
char build_date[25];
|
||||
};
|
||||
|
||||
struct masterboard_data {
|
||||
int digitalInputBits;
|
||||
int digitalOutputBits;
|
||||
char analogInputRange0;
|
||||
char analogInputRange1;
|
||||
double analogInput0;
|
||||
double analogInput1;
|
||||
char analogOutputDomain0;
|
||||
char analogOutputDomain1;
|
||||
double analogOutput0;
|
||||
double analogOutput1;
|
||||
float masterBoardTemperature;
|
||||
float robotVoltage48V;
|
||||
float robotCurrent;
|
||||
float masterIOCurrent;
|
||||
unsigned char safetyMode;
|
||||
unsigned char masterOnOffState;
|
||||
char euromap67InterfaceInstalled;
|
||||
int euromapInputBits;
|
||||
int euromapOutputBits;
|
||||
float euromapVoltage;
|
||||
float euromapCurrent;
|
||||
struct masterboard_data
|
||||
{
|
||||
int digitalInputBits;
|
||||
int digitalOutputBits;
|
||||
char analogInputRange0;
|
||||
char analogInputRange1;
|
||||
double analogInput0;
|
||||
double analogInput1;
|
||||
char analogOutputDomain0;
|
||||
char analogOutputDomain1;
|
||||
double analogOutput0;
|
||||
double analogOutput1;
|
||||
float masterBoardTemperature;
|
||||
float robotVoltage48V;
|
||||
float robotCurrent;
|
||||
float masterIOCurrent;
|
||||
unsigned char safetyMode;
|
||||
unsigned char masterOnOffState;
|
||||
char euromap67InterfaceInstalled;
|
||||
int euromapInputBits;
|
||||
int euromapOutputBits;
|
||||
float euromapVoltage;
|
||||
float euromapCurrent;
|
||||
};
|
||||
|
||||
struct robot_mode_data {
|
||||
uint64_t timestamp;
|
||||
bool isRobotConnected;
|
||||
bool isRealRobotEnabled;
|
||||
bool isPowerOnRobot;
|
||||
bool isEmergencyStopped;
|
||||
bool isProtectiveStopped;
|
||||
bool isProgramRunning;
|
||||
bool isProgramPaused;
|
||||
unsigned char robotMode;
|
||||
unsigned char controlMode;
|
||||
double targetSpeedFraction;
|
||||
double speedScaling;
|
||||
struct robot_mode_data
|
||||
{
|
||||
uint64_t timestamp;
|
||||
bool isRobotConnected;
|
||||
bool isRealRobotEnabled;
|
||||
bool isPowerOnRobot;
|
||||
bool isEmergencyStopped;
|
||||
bool isProtectiveStopped;
|
||||
bool isProgramRunning;
|
||||
bool isProgramPaused;
|
||||
unsigned char robotMode;
|
||||
unsigned char controlMode;
|
||||
double targetSpeedFraction;
|
||||
double speedScaling;
|
||||
};
|
||||
|
||||
class RobotState {
|
||||
class RobotState
|
||||
{
|
||||
private:
|
||||
version_message version_msg_;
|
||||
masterboard_data mb_data_;
|
||||
robot_mode_data robot_mode_;
|
||||
version_message version_msg_;
|
||||
masterboard_data mb_data_;
|
||||
robot_mode_data robot_mode_;
|
||||
|
||||
std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
|
||||
std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
|
||||
|
||||
std::condition_variable* pMsg_cond_; //Signals that new vars are available
|
||||
bool new_data_available_; //to avoid spurious wakes
|
||||
unsigned char robot_mode_running_;
|
||||
std::condition_variable* pMsg_cond_; // Signals that new vars are available
|
||||
bool new_data_available_; // to avoid spurious wakes
|
||||
unsigned char robot_mode_running_;
|
||||
|
||||
double ntohd(uint64_t nf);
|
||||
double ntohd(uint64_t nf);
|
||||
|
||||
public:
|
||||
RobotState(std::condition_variable& msg_cond);
|
||||
~RobotState();
|
||||
double getVersion();
|
||||
double getTime();
|
||||
std::vector<double> getQTarget();
|
||||
int getDigitalInputBits();
|
||||
int getDigitalOutputBits();
|
||||
char getAnalogInputRange0();
|
||||
char getAnalogInputRange1();
|
||||
double getAnalogInput0();
|
||||
double getAnalogInput1();
|
||||
char getAnalogOutputDomain0();
|
||||
char getAnalogOutputDomain1();
|
||||
double getAnalogOutput0();
|
||||
double getAnalogOutput1();
|
||||
std::vector<double> getVActual();
|
||||
float getMasterBoardTemperature();
|
||||
float getRobotVoltage48V();
|
||||
float getRobotCurrent();
|
||||
float getMasterIOCurrent();
|
||||
unsigned char getSafetyMode();
|
||||
unsigned char getInReducedMode();
|
||||
char getEuromap67InterfaceInstalled();
|
||||
int getEuromapInputBits();
|
||||
int getEuromapOutputBits();
|
||||
float getEuromapVoltage();
|
||||
float getEuromapCurrent();
|
||||
RobotState(std::condition_variable& msg_cond);
|
||||
~RobotState();
|
||||
double getVersion();
|
||||
double getTime();
|
||||
std::vector<double> getQTarget();
|
||||
int getDigitalInputBits();
|
||||
int getDigitalOutputBits();
|
||||
char getAnalogInputRange0();
|
||||
char getAnalogInputRange1();
|
||||
double getAnalogInput0();
|
||||
double getAnalogInput1();
|
||||
char getAnalogOutputDomain0();
|
||||
char getAnalogOutputDomain1();
|
||||
double getAnalogOutput0();
|
||||
double getAnalogOutput1();
|
||||
std::vector<double> getVActual();
|
||||
float getMasterBoardTemperature();
|
||||
float getRobotVoltage48V();
|
||||
float getRobotCurrent();
|
||||
float getMasterIOCurrent();
|
||||
unsigned char getSafetyMode();
|
||||
unsigned char getInReducedMode();
|
||||
char getEuromap67InterfaceInstalled();
|
||||
int getEuromapInputBits();
|
||||
int getEuromapOutputBits();
|
||||
float getEuromapVoltage();
|
||||
float getEuromapCurrent();
|
||||
|
||||
bool isRobotConnected();
|
||||
bool isRealRobotEnabled();
|
||||
bool isPowerOnRobot();
|
||||
bool isEmergencyStopped();
|
||||
bool isProtectiveStopped();
|
||||
bool isProgramRunning();
|
||||
bool isProgramPaused();
|
||||
unsigned char getRobotMode();
|
||||
bool isReady();
|
||||
bool isRobotConnected();
|
||||
bool isRealRobotEnabled();
|
||||
bool isPowerOnRobot();
|
||||
bool isEmergencyStopped();
|
||||
bool isProtectiveStopped();
|
||||
bool isProgramRunning();
|
||||
bool isProgramPaused();
|
||||
unsigned char getRobotMode();
|
||||
bool isReady();
|
||||
|
||||
void setDisconnected();
|
||||
void setDisconnected();
|
||||
|
||||
bool getNewDataAvailable();
|
||||
void finishedReading();
|
||||
bool getNewDataAvailable();
|
||||
void finishedReading();
|
||||
|
||||
void unpack(uint8_t* buf, unsigned int buf_length);
|
||||
void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
|
||||
uint32_t len);
|
||||
void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
|
||||
void unpackRobotMode(uint8_t* buf, unsigned int offset);
|
||||
void unpack(uint8_t* buf, unsigned int buf_length);
|
||||
void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
|
||||
void unpackRobotMode(uint8_t* buf, unsigned int offset);
|
||||
};
|
||||
|
||||
#endif /* ROBOT_STATE_H_ */
|
||||
|
||||
@@ -19,98 +19,101 @@
|
||||
#ifndef ROBOT_STATE_RT_H_
|
||||
#define ROBOT_STATE_RT_H_
|
||||
|
||||
#include <condition_variable>
|
||||
#include <inttypes.h>
|
||||
#include <mutex>
|
||||
#include <netinet/in.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
class RobotStateRT {
|
||||
class RobotStateRT
|
||||
{
|
||||
private:
|
||||
double version_; //protocol version
|
||||
double version_; // protocol version
|
||||
|
||||
double time_; //Time elapsed since the controller was started
|
||||
std::vector<double> q_target_; //Target joint positions
|
||||
std::vector<double> qd_target_; //Target joint velocities
|
||||
std::vector<double> qdd_target_; //Target joint accelerations
|
||||
std::vector<double> i_target_; //Target joint currents
|
||||
std::vector<double> m_target_; //Target joint moments (torques)
|
||||
std::vector<double> q_actual_; //Actual joint positions
|
||||
std::vector<double> qd_actual_; //Actual joint velocities
|
||||
std::vector<double> i_actual_; //Actual joint currents
|
||||
std::vector<double> i_control_; //Joint control currents
|
||||
std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
|
||||
std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
|
||||
std::vector<double> tcp_force_; //Generalised forces in the TC
|
||||
std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
|
||||
std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
|
||||
std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
|
||||
std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
|
||||
double controller_timer_; //Controller realtime thread execution time
|
||||
double robot_mode_; //Robot mode
|
||||
std::vector<double> joint_modes_; //Joint control modes
|
||||
double safety_mode_; //Safety mode
|
||||
std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
|
||||
double speed_scaling_; //Speed scaling of the trajectory limiter
|
||||
double linear_momentum_norm_; //Norm of Cartesian linear momentum
|
||||
double v_main_; //Masterboard: Main voltage
|
||||
double v_robot_; //Matorborad: Robot voltage (48V)
|
||||
double i_robot_; //Masterboard: Robot current
|
||||
std::vector<double> v_actual_; //Actual joint voltages
|
||||
double time_; // Time elapsed since the controller was started
|
||||
std::vector<double> q_target_; // Target joint positions
|
||||
std::vector<double> qd_target_; // Target joint velocities
|
||||
std::vector<double> qdd_target_; // Target joint accelerations
|
||||
std::vector<double> i_target_; // Target joint currents
|
||||
std::vector<double> m_target_; // Target joint moments (torques)
|
||||
std::vector<double> q_actual_; // Actual joint positions
|
||||
std::vector<double> qd_actual_; // Actual joint velocities
|
||||
std::vector<double> i_actual_; // Actual joint currents
|
||||
std::vector<double> i_control_; // Joint control currents
|
||||
std::vector<double> tool_vector_actual_; // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
|
||||
// and rz is a rotation vector representation of the tool orientation
|
||||
std::vector<double> tcp_speed_actual_; // Actual speed of the tool given in Cartesian coordinates
|
||||
std::vector<double> tcp_force_; // Generalised forces in the TC
|
||||
std::vector<double> tool_vector_target_; // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
|
||||
// and rz is a rotation vector representation of the tool orientation
|
||||
std::vector<double> tcp_speed_target_; // Target speed of the tool given in Cartesian coordinates
|
||||
std::vector<bool> digital_input_bits_; // Current state of the digital inputs. NOTE: these are bits encoded as
|
||||
// int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
|
||||
std::vector<double> motor_temperatures_; // Temperature of each joint in degrees celsius
|
||||
double controller_timer_; // Controller realtime thread execution time
|
||||
double robot_mode_; // Robot mode
|
||||
std::vector<double> joint_modes_; // Joint control modes
|
||||
double safety_mode_; // Safety mode
|
||||
std::vector<double> tool_accelerometer_values_; // Tool x,y and z accelerometer values (software version 1.7)
|
||||
double speed_scaling_; // Speed scaling of the trajectory limiter
|
||||
double linear_momentum_norm_; // Norm of Cartesian linear momentum
|
||||
double v_main_; // Masterboard: Main voltage
|
||||
double v_robot_; // Matorborad: Robot voltage (48V)
|
||||
double i_robot_; // Masterboard: Robot current
|
||||
std::vector<double> v_actual_; // Actual joint voltages
|
||||
|
||||
std::mutex val_lock_; // Locks the variables while unpack parses data;
|
||||
std::mutex val_lock_; // Locks the variables while unpack parses data;
|
||||
|
||||
std::condition_variable* pMsg_cond_; //Signals that new vars are available
|
||||
bool data_published_; //to avoid spurious wakes
|
||||
bool controller_updated_; //to avoid spurious wakes
|
||||
std::condition_variable* pMsg_cond_; // Signals that new vars are available
|
||||
bool data_published_; // to avoid spurious wakes
|
||||
bool controller_updated_; // to avoid spurious wakes
|
||||
|
||||
std::vector<double> unpackVector(uint8_t* buf, int start_index,
|
||||
int nr_of_vals);
|
||||
std::vector<bool> unpackDigitalInputBits(int64_t data);
|
||||
double ntohd(uint64_t nf);
|
||||
std::vector<double> unpackVector(uint8_t* buf, int start_index, int nr_of_vals);
|
||||
std::vector<bool> unpackDigitalInputBits(int64_t data);
|
||||
double ntohd(uint64_t nf);
|
||||
|
||||
public:
|
||||
RobotStateRT(std::condition_variable& msg_cond);
|
||||
~RobotStateRT();
|
||||
double getVersion();
|
||||
double getTime();
|
||||
std::vector<double> getQTarget();
|
||||
std::vector<double> getQdTarget();
|
||||
std::vector<double> getQddTarget();
|
||||
std::vector<double> getITarget();
|
||||
std::vector<double> getMTarget();
|
||||
std::vector<double> getQActual();
|
||||
std::vector<double> getQdActual();
|
||||
std::vector<double> getIActual();
|
||||
std::vector<double> getIControl();
|
||||
std::vector<double> getToolVectorActual();
|
||||
std::vector<double> getTcpSpeedActual();
|
||||
std::vector<double> getTcpForce();
|
||||
std::vector<double> getToolVectorTarget();
|
||||
std::vector<double> getTcpSpeedTarget();
|
||||
std::vector<bool> getDigitalInputBits();
|
||||
std::vector<double> getMotorTemperatures();
|
||||
double getControllerTimer();
|
||||
double getRobotMode();
|
||||
std::vector<double> getJointModes();
|
||||
double getSafety_mode();
|
||||
std::vector<double> getToolAccelerometerValues();
|
||||
double getSpeedScaling();
|
||||
double getLinearMomentumNorm();
|
||||
double getVMain();
|
||||
double getVRobot();
|
||||
double getIRobot();
|
||||
RobotStateRT(std::condition_variable& msg_cond);
|
||||
~RobotStateRT();
|
||||
double getVersion();
|
||||
double getTime();
|
||||
std::vector<double> getQTarget();
|
||||
std::vector<double> getQdTarget();
|
||||
std::vector<double> getQddTarget();
|
||||
std::vector<double> getITarget();
|
||||
std::vector<double> getMTarget();
|
||||
std::vector<double> getQActual();
|
||||
std::vector<double> getQdActual();
|
||||
std::vector<double> getIActual();
|
||||
std::vector<double> getIControl();
|
||||
std::vector<double> getToolVectorActual();
|
||||
std::vector<double> getTcpSpeedActual();
|
||||
std::vector<double> getTcpForce();
|
||||
std::vector<double> getToolVectorTarget();
|
||||
std::vector<double> getTcpSpeedTarget();
|
||||
std::vector<bool> getDigitalInputBits();
|
||||
std::vector<double> getMotorTemperatures();
|
||||
double getControllerTimer();
|
||||
double getRobotMode();
|
||||
std::vector<double> getJointModes();
|
||||
double getSafety_mode();
|
||||
std::vector<double> getToolAccelerometerValues();
|
||||
double getSpeedScaling();
|
||||
double getLinearMomentumNorm();
|
||||
double getVMain();
|
||||
double getVRobot();
|
||||
double getIRobot();
|
||||
|
||||
void setVersion(double ver);
|
||||
void setVersion(double ver);
|
||||
|
||||
void setDataPublished();
|
||||
bool getDataPublished();
|
||||
bool getControllerUpdated();
|
||||
void setControllerUpdated();
|
||||
std::vector<double> getVActual();
|
||||
void unpack(uint8_t* buf);
|
||||
void setDataPublished();
|
||||
bool getDataPublished();
|
||||
bool getControllerUpdated();
|
||||
void setControllerUpdated();
|
||||
std::vector<double> getVActual();
|
||||
void unpack(uint8_t* buf);
|
||||
};
|
||||
|
||||
#endif /* ROBOT_STATE_RT_H_ */
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/WrenchStamped.h>
|
||||
@@ -9,6 +8,7 @@
|
||||
#include <sensor_msgs/Temperature.h>
|
||||
#include <tf/tf.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <cstdlib>
|
||||
#include <vector>
|
||||
|
||||
#include "ur_modern_driver/ur/consumer.h"
|
||||
@@ -16,55 +16,57 @@
|
||||
using namespace ros;
|
||||
using namespace tf;
|
||||
|
||||
const std::string JOINTS[] = {
|
||||
"shoulder_pan_joint",
|
||||
"shoulder_lift_joint",
|
||||
"elbow_joint",
|
||||
"wrist_1_joint",
|
||||
"wrist_2_joint",
|
||||
"wrist_3_joint"
|
||||
};
|
||||
const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
|
||||
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
|
||||
|
||||
class RTPublisher : public URRTPacketConsumer {
|
||||
class RTPublisher : public URRTPacketConsumer
|
||||
{
|
||||
private:
|
||||
NodeHandle _nh;
|
||||
Publisher _joint_pub;
|
||||
Publisher _wrench_pub;
|
||||
Publisher _tool_vel_pub;
|
||||
Publisher _joint_temperature_pub;
|
||||
TransformBroadcaster _transform_broadcaster;
|
||||
std::vector<std::string> _joint_names;
|
||||
std::string _base_frame;
|
||||
std::string _tool_frame;
|
||||
NodeHandle nh_;
|
||||
Publisher joint_pub_;
|
||||
Publisher wrench_pub_;
|
||||
Publisher tool_vel_pub_;
|
||||
Publisher joint_temperature_pub_;
|
||||
TransformBroadcaster transform_broadcaster_;
|
||||
std::vector<std::string> joint_names_;
|
||||
std::string base_frame_;
|
||||
std::string tool_frame_;
|
||||
|
||||
bool publish_joints(RTShared& packet, Time& t);
|
||||
bool publish_wrench(RTShared& packet, Time& t);
|
||||
bool publish_tool(RTShared& packet, Time& t);
|
||||
bool publish_transform(RTShared& packet, Time& t);
|
||||
bool publish_temperature(RTShared& packet, Time& t);
|
||||
bool publishJoints(RTShared& packet, Time& t);
|
||||
bool publishWrench(RTShared& packet, Time& t);
|
||||
bool publishTool(RTShared& packet, Time& t);
|
||||
bool publishTransform(RTShared& packet, Time& t);
|
||||
bool publishTemperature(RTShared& packet, Time& t);
|
||||
|
||||
bool publish(RTShared& packet);
|
||||
bool publish(RTShared& packet);
|
||||
|
||||
public:
|
||||
RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
|
||||
: _joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1))
|
||||
, _wrench_pub(_nh.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
|
||||
, _tool_vel_pub(_nh.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
|
||||
, _joint_temperature_pub(_nh.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
|
||||
, _base_frame(base_frame)
|
||||
, _tool_frame(tool_frame)
|
||||
RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
|
||||
: joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
|
||||
, wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
|
||||
, tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
|
||||
, joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
|
||||
, base_frame_(base_frame)
|
||||
, tool_frame_(tool_frame)
|
||||
{
|
||||
for (auto const& j : JOINTS)
|
||||
{
|
||||
for (auto const& j : JOINTS) {
|
||||
_joint_names.push_back(joint_prefix + j);
|
||||
}
|
||||
joint_names_.push_back(joint_prefix + j);
|
||||
}
|
||||
}
|
||||
|
||||
virtual bool consume(RTState_V1_6__7& state);
|
||||
virtual bool consume(RTState_V1_8& state);
|
||||
virtual bool consume(RTState_V3_0__1& state);
|
||||
virtual bool consume(RTState_V3_2__3& state);
|
||||
virtual bool consume(RTState_V1_6__7& state);
|
||||
virtual bool consume(RTState_V1_8& state);
|
||||
virtual bool consume(RTState_V3_0__1& state);
|
||||
virtual bool consume(RTState_V3_2__3& state);
|
||||
|
||||
virtual void setup_consumer() {}
|
||||
virtual void teardown_consumer() {}
|
||||
virtual void stop_consumer() {}
|
||||
virtual void setupConsumer()
|
||||
{
|
||||
}
|
||||
virtual void teardownConsumer()
|
||||
{
|
||||
}
|
||||
virtual void stopConsumer()
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -2,21 +2,23 @@
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
struct double3_t {
|
||||
double x, y, z;
|
||||
struct double3_t
|
||||
{
|
||||
double x, y, z;
|
||||
};
|
||||
|
||||
struct cartesian_coord_t {
|
||||
double3_t position;
|
||||
double3_t rotation;
|
||||
struct cartesian_coord_t
|
||||
{
|
||||
double3_t position;
|
||||
double3_t rotation;
|
||||
};
|
||||
|
||||
inline bool operator==(const double3_t& lhs, const double3_t& rhs)
|
||||
{
|
||||
return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
|
||||
return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z;
|
||||
}
|
||||
|
||||
inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs)
|
||||
{
|
||||
return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
|
||||
return lhs.position == rhs.position && lhs.rotation == rhs.rotation;
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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;
|
||||
}
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
};
|
||||
@@ -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");
|
||||
};
|
||||
@@ -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;
|
||||
|
||||
@@ -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!");
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
@@ -19,13 +19,7 @@
|
||||
#ifndef UR_COMMUNICATION_H_
|
||||
#define UR_COMMUNICATION_H_
|
||||
|
||||
#include "do_output.h"
|
||||
#include "robot_state.h"
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <fcntl.h>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
@@ -36,27 +30,34 @@
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/types.h>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "do_output.h"
|
||||
#include "robot_state.h"
|
||||
|
||||
class UrCommunication {
|
||||
class UrCommunication
|
||||
{
|
||||
private:
|
||||
int pri_sockfd_, sec_sockfd_;
|
||||
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
|
||||
struct hostent* server_;
|
||||
bool keepalive_;
|
||||
std::thread comThread_;
|
||||
int flag_;
|
||||
void run();
|
||||
int pri_sockfd_, sec_sockfd_;
|
||||
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
|
||||
struct hostent* server_;
|
||||
bool keepalive_;
|
||||
std::thread comThread_;
|
||||
int flag_;
|
||||
void run();
|
||||
|
||||
public:
|
||||
bool connected_;
|
||||
RobotState* robot_state_;
|
||||
bool connected_;
|
||||
RobotState* robot_state_;
|
||||
|
||||
UrCommunication(std::condition_variable& msg_cond, std::string host);
|
||||
bool start();
|
||||
void halt();
|
||||
UrCommunication(std::condition_variable& msg_cond, std::string host);
|
||||
bool start();
|
||||
void halt();
|
||||
};
|
||||
|
||||
#endif /* UR_COMMUNICATION_H_ */
|
||||
|
||||
@@ -19,81 +19,79 @@
|
||||
#ifndef UR_DRIVER_H_
|
||||
#define UR_DRIVER_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "do_output.h"
|
||||
#include "ur_communication.h"
|
||||
#include "ur_realtime_communication.h"
|
||||
#include <condition_variable>
|
||||
#include <math.h>
|
||||
#include <mutex>
|
||||
#include <netinet/in.h>
|
||||
#include <string>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <vector>
|
||||
|
||||
#include <chrono>
|
||||
|
||||
class UrDriver {
|
||||
class UrDriver
|
||||
{
|
||||
private:
|
||||
double maximum_time_step_;
|
||||
double minimum_payload_;
|
||||
double maximum_payload_;
|
||||
std::vector<std::string> joint_names_;
|
||||
std::string ip_addr_;
|
||||
const int MULT_JOINTSTATE_ = 1000000;
|
||||
const int MULT_TIME_ = 1000000;
|
||||
const unsigned int REVERSE_PORT_;
|
||||
int incoming_sockfd_;
|
||||
int new_sockfd_;
|
||||
bool reverse_connected_;
|
||||
double servoj_time_;
|
||||
bool executing_traj_;
|
||||
double firmware_version_;
|
||||
double servoj_lookahead_time_;
|
||||
double servoj_gain_;
|
||||
double maximum_time_step_;
|
||||
double minimum_payload_;
|
||||
double maximum_payload_;
|
||||
std::vector<std::string> joint_names_;
|
||||
std::string ip_addr_;
|
||||
const int MULT_JOINTSTATE_ = 1000000;
|
||||
const int MULT_TIME_ = 1000000;
|
||||
const unsigned int REVERSE_PORT_;
|
||||
int incoming_sockfd_;
|
||||
int new_sockfd_;
|
||||
bool reverse_connected_;
|
||||
double servoj_time_;
|
||||
bool executing_traj_;
|
||||
double firmware_version_;
|
||||
double servoj_lookahead_time_;
|
||||
double servoj_gain_;
|
||||
|
||||
public:
|
||||
UrRealtimeCommunication* rt_interface_;
|
||||
UrCommunication* sec_interface_;
|
||||
UrRealtimeCommunication* rt_interface_;
|
||||
UrCommunication* sec_interface_;
|
||||
|
||||
UrDriver(std::condition_variable& rt_msg_cond,
|
||||
std::condition_variable& msg_cond, std::string host,
|
||||
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0.,
|
||||
double max_payload = 1., double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
|
||||
bool start();
|
||||
void halt();
|
||||
UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
|
||||
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12,
|
||||
double max_time_step = 0.08, double min_payload = 0., double max_payload = 1.,
|
||||
double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
|
||||
bool start();
|
||||
void halt();
|
||||
|
||||
void setSpeed(double q0, double q1, double q2, double q3, double q4,
|
||||
double q5, double acc = 100.);
|
||||
void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
|
||||
|
||||
bool doTraj(std::vector<double> inp_timestamps,
|
||||
std::vector<std::vector<double> > inp_positions,
|
||||
std::vector<std::vector<double> > inp_velocities);
|
||||
void servoj(std::vector<double> positions, int keepalive = 1);
|
||||
bool doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
|
||||
std::vector<std::vector<double>> inp_velocities);
|
||||
void servoj(std::vector<double> positions, int keepalive = 1);
|
||||
|
||||
void stopTraj();
|
||||
void stopTraj();
|
||||
|
||||
bool uploadProg();
|
||||
bool openServo();
|
||||
void closeServo(std::vector<double> positions);
|
||||
bool uploadProg();
|
||||
bool openServo();
|
||||
void closeServo(std::vector<double> positions);
|
||||
|
||||
std::vector<double> interp_cubic(double t, double T,
|
||||
std::vector<double> p0_pos, std::vector<double> p1_pos,
|
||||
std::vector<double> p0_vel, std::vector<double> p1_vel);
|
||||
std::vector<double> interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
|
||||
std::vector<double> p0_vel, std::vector<double> p1_vel);
|
||||
|
||||
std::vector<std::string> getJointNames();
|
||||
void setJointNames(std::vector<std::string> jn);
|
||||
void setToolVoltage(unsigned int v);
|
||||
void setFlag(unsigned int n, bool b);
|
||||
void setDigitalOut(unsigned int n, bool b);
|
||||
void setAnalogOut(unsigned int n, double f);
|
||||
bool setPayload(double m);
|
||||
std::vector<std::string> getJointNames();
|
||||
void setJointNames(std::vector<std::string> jn);
|
||||
void setToolVoltage(unsigned int v);
|
||||
void setFlag(unsigned int n, bool b);
|
||||
void setDigitalOut(unsigned int n, bool b);
|
||||
void setAnalogOut(unsigned int n, double f);
|
||||
bool setPayload(double m);
|
||||
|
||||
void setMinPayload(double m);
|
||||
void setMaxPayload(double m);
|
||||
void setServojTime(double t);
|
||||
void setServojLookahead(double t);
|
||||
void setServojGain(double g);
|
||||
void setMinPayload(double m);
|
||||
void setMaxPayload(double m);
|
||||
void setServojTime(double t);
|
||||
void setServojLookahead(double t);
|
||||
void setServojGain(double g);
|
||||
};
|
||||
|
||||
#endif /* UR_DRIVER_H_ */
|
||||
|
||||
@@ -58,9 +58,6 @@
|
||||
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
|
||||
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
|
||||
|
||||
#include "do_output.h"
|
||||
#include "ur_driver.h"
|
||||
#include <boost/scoped_ptr.hpp>
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <hardware_interface/force_torque_sensor_interface.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
@@ -68,69 +65,72 @@
|
||||
#include <hardware_interface/robot_hw.h>
|
||||
#include <math.h>
|
||||
#include <ros/ros.h>
|
||||
#include <boost/scoped_ptr.hpp>
|
||||
#include "do_output.h"
|
||||
#include "ur_driver.h"
|
||||
|
||||
namespace ros_control_ur {
|
||||
|
||||
namespace ros_control_ur
|
||||
{
|
||||
// For simulation only - determines how fast a trajectory is followed
|
||||
static const double POSITION_STEP_FACTOR = 1;
|
||||
static const double VELOCITY_STEP_FACTOR = 1;
|
||||
|
||||
/// \brief Hardware interface for a robot
|
||||
class UrHardwareInterface : public hardware_interface::RobotHW {
|
||||
class UrHardwareInterface : public hardware_interface::RobotHW
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* \brief Constructor
|
||||
* \param nh - Node handle for topics.
|
||||
*/
|
||||
UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
|
||||
/**
|
||||
* \brief Constructor
|
||||
* \param nh - Node handle for topics.
|
||||
*/
|
||||
UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
|
||||
|
||||
/// \brief Initialize the hardware interface
|
||||
virtual void init();
|
||||
/// \brief Initialize the hardware interface
|
||||
virtual void init();
|
||||
|
||||
/// \brief Read the state from the robot hardware.
|
||||
virtual void read();
|
||||
/// \brief Read the state from the robot hardware.
|
||||
virtual void read();
|
||||
|
||||
/// \brief write the command to the robot hardware.
|
||||
virtual void write();
|
||||
/// \brief write the command to the robot hardware.
|
||||
virtual void write();
|
||||
|
||||
void setMaxVelChange(double inp);
|
||||
void setMaxVelChange(double inp);
|
||||
|
||||
bool canSwitch(
|
||||
const std::list<hardware_interface::ControllerInfo>& start_list,
|
||||
const std::list<hardware_interface::ControllerInfo>& stop_list) const;
|
||||
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
|
||||
const std::list<hardware_interface::ControllerInfo>& stop_list);
|
||||
bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
|
||||
const std::list<hardware_interface::ControllerInfo>& stop_list) const;
|
||||
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
|
||||
const std::list<hardware_interface::ControllerInfo>& stop_list);
|
||||
|
||||
protected:
|
||||
// Startup and shutdown of the internal node inside a roscpp program
|
||||
ros::NodeHandle nh_;
|
||||
// Startup and shutdown of the internal node inside a roscpp program
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
// Interfaces
|
||||
hardware_interface::JointStateInterface joint_state_interface_;
|
||||
hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
|
||||
hardware_interface::PositionJointInterface position_joint_interface_;
|
||||
hardware_interface::VelocityJointInterface velocity_joint_interface_;
|
||||
bool velocity_interface_running_;
|
||||
bool position_interface_running_;
|
||||
// Shared memory
|
||||
std::vector<std::string> joint_names_;
|
||||
std::vector<double> joint_position_;
|
||||
std::vector<double> joint_velocity_;
|
||||
std::vector<double> joint_effort_;
|
||||
std::vector<double> joint_position_command_;
|
||||
std::vector<double> joint_velocity_command_;
|
||||
std::vector<double> prev_joint_velocity_command_;
|
||||
std::size_t num_joints_;
|
||||
double robot_force_[3] = { 0., 0., 0. };
|
||||
double robot_torque_[3] = { 0., 0., 0. };
|
||||
// Interfaces
|
||||
hardware_interface::JointStateInterface joint_state_interface_;
|
||||
hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
|
||||
hardware_interface::PositionJointInterface position_joint_interface_;
|
||||
hardware_interface::VelocityJointInterface velocity_joint_interface_;
|
||||
bool velocity_interface_running_;
|
||||
bool position_interface_running_;
|
||||
// Shared memory
|
||||
std::vector<std::string> joint_names_;
|
||||
std::vector<double> joint_position_;
|
||||
std::vector<double> joint_velocity_;
|
||||
std::vector<double> joint_effort_;
|
||||
std::vector<double> joint_position_command_;
|
||||
std::vector<double> joint_velocity_command_;
|
||||
std::vector<double> prev_joint_velocity_command_;
|
||||
std::size_t num_joints_;
|
||||
double robot_force_[3] = { 0., 0., 0. };
|
||||
double robot_torque_[3] = { 0., 0., 0. };
|
||||
|
||||
double max_vel_change_;
|
||||
double max_vel_change_;
|
||||
|
||||
// Robot API
|
||||
UrDriver* robot_;
|
||||
// Robot API
|
||||
UrDriver* robot_;
|
||||
};
|
||||
// class
|
||||
|
||||
} // namespace
|
||||
} // namespace
|
||||
|
||||
#endif
|
||||
|
||||
@@ -19,14 +19,9 @@
|
||||
#ifndef UR_REALTIME_COMMUNICATION_H_
|
||||
#define UR_REALTIME_COMMUNICATION_H_
|
||||
|
||||
#include "do_output.h"
|
||||
#include "robot_state_RT.h"
|
||||
#include <arpa/inet.h>
|
||||
#include <condition_variable>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
@@ -37,38 +32,42 @@
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/types.h>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <condition_variable>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "do_output.h"
|
||||
#include "robot_state_RT.h"
|
||||
|
||||
class UrRealtimeCommunication {
|
||||
class UrRealtimeCommunication
|
||||
{
|
||||
private:
|
||||
unsigned int safety_count_max_;
|
||||
int sockfd_;
|
||||
struct sockaddr_in serv_addr_;
|
||||
struct hostent* server_;
|
||||
std::string local_ip_;
|
||||
bool keepalive_;
|
||||
std::thread comThread_;
|
||||
int flag_;
|
||||
std::recursive_mutex command_string_lock_;
|
||||
std::string command_;
|
||||
unsigned int safety_count_;
|
||||
void run();
|
||||
unsigned int safety_count_max_;
|
||||
int sockfd_;
|
||||
struct sockaddr_in serv_addr_;
|
||||
struct hostent* server_;
|
||||
std::string local_ip_;
|
||||
bool keepalive_;
|
||||
std::thread comThread_;
|
||||
int flag_;
|
||||
std::recursive_mutex command_string_lock_;
|
||||
std::string command_;
|
||||
unsigned int safety_count_;
|
||||
void run();
|
||||
|
||||
public:
|
||||
bool connected_;
|
||||
RobotStateRT* robot_state_;
|
||||
bool connected_;
|
||||
RobotStateRT* robot_state_;
|
||||
|
||||
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
|
||||
unsigned int safety_count_max = 12);
|
||||
bool start();
|
||||
void halt();
|
||||
void setSpeed(double q0, double q1, double q2, double q3, double q4,
|
||||
double q5, double acc = 100.);
|
||||
void addCommandToQueue(std::string inp);
|
||||
void setSafetyCountMax(uint inp);
|
||||
std::string getLocalIp();
|
||||
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12);
|
||||
bool start();
|
||||
void halt();
|
||||
void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
|
||||
void addCommandToQueue(std::string inp);
|
||||
void setSafetyCountMax(uint inp);
|
||||
std::string getLocalIp();
|
||||
};
|
||||
|
||||
#endif /* UR_REALTIME_COMMUNICATION_H_ */
|
||||
|
||||
Reference in New Issue
Block a user