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

Added clang formatting

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

137
include/ur_modern_driver/bin_parser.h
View File

@@ -1,146 +1,171 @@
#pragma once
#include <inttypes.h>
#include <endian.h>
#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 <string>
#include <assert.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/log.h"
class BinParser {
private:
uint8_t *_buf_pos, *_buf_end;
BinParser &_parent;
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(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(uint8_t* buffer, size_t buf_len)
: _buf_pos(buffer)
, _buf_end(buffer + buf_len)
, _parent(*this)
{
assert(_buf_pos <= _buf_end);
}
~BinParser() {
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;
}
}
//Decode from network encoding (big endian) to host encoding
template<typename T>
T decode(T val) {
template <typename T>
T decode(T val)
{
return val;
}
uint16_t decode(uint16_t val) {
uint16_t decode(uint16_t val)
{
return be16toh(val);
}
uint32_t decode(uint32_t val) {
uint32_t decode(uint32_t val)
{
return be32toh(val);
}
uint64_t decode(uint64_t val) {
uint64_t decode(uint64_t val)
{
return be64toh(val);
}
int16_t decode(int16_t val) {
int16_t decode(int16_t val)
{
return be16toh(val);
}
int32_t decode(int32_t val) {
int32_t decode(int32_t val)
{
return be32toh(val);
}
int64_t decode(int64_t val) {
int64_t decode(int64_t val)
{
return be64toh(val);
}
float decode(float val) {
float decode(float val)
{
return be32toh(val);
}
double decode(double val) {
double decode(double val)
{
return be64toh(val);
}
template<typename T>
T peek() {
template <typename T>
T peek()
{
assert(_buf_pos <= _buf_end);
return decode(*(reinterpret_cast<T*>(_buf_pos)));
}
template<typename T>
void parse(T &val) {
template <typename T>
void parse(T& val)
{
val = peek<T>();
_buf_pos += sizeof(T);
}
// UR uses 1 byte for boolean values but sizeof(bool) is implementation
// 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) {
void parse(bool& val)
{
uint8_t inner;
parse<uint8_t>(inner);
val = inner != 0;
val = inner != 0;
}
// Explicit parsing order of fields to avoid issues with struct layout
void parse(double3_t &val) {
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) {
void parse(cartesian_coord_t& val)
{
parse(val.position);
parse(val.rotation);
}
void parse_remainder(std::string &val) {
void parse_remainder(std::string& val)
{
parse(val, size_t(_buf_end - _buf_pos));
}
void parse(std::string &val, size_t 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) {
// 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]) {
for(size_t i = 0; i < N; i++) {
template <typename T, size_t N>
void parse(T (&array)[N])
{
for (size_t i = 0; i < N; i++) {
parse(array[i]);
}
}
void consume() {
void consume()
{
_buf_pos = _buf_end;
}
void consume(size_t bytes) {
void consume(size_t bytes)
{
_buf_pos += bytes;
}
bool check_size(size_t bytes) {
bool check_size(size_t bytes)
{
return bytes <= size_t(_buf_end - _buf_pos);
}
template<typename T>
bool check_size(void) {
template <typename T>
bool check_size(void)
{
return check_size(T::SIZE);
}
bool empty() {
bool empty()
{
return _buf_pos == _buf_end;
}
void debug() {
void debug()
{
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos);
}
};

1
include/ur_modern_driver/do_output.h
View File

@@ -30,5 +30,4 @@ void print_warning(std::string inp);
void print_error(std::string inp);
void print_fatal(std::string inp);
#endif /* UR_DO_OUTPUT_H_ */

22
include/ur_modern_driver/log.h
View File

@@ -2,20 +2,20 @@
#include <inttypes.h>
#ifdef ROS_BUILD
#include <ros/ros.h>
#include <ros/ros.h>
#define LOG_DEBUG ROS_DEBUG
#define LOG_WARN ROS_WARN
#define LOG_INFO ROS_INFO
#define LOG_ERROR ROS_ERROR
#define LOG_FATAL ROS_FATAL
#define LOG_DEBUG ROS_DEBUG
#define LOG_WARN ROS_WARN
#define LOG_INFO ROS_INFO
#define LOG_ERROR ROS_ERROR
#define LOG_FATAL ROS_FATAL
#else
#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__)
#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__)
#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__)
#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__)
#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__)
#define LOG_DEBUG(format, ...) printf("[DEBUG]: " format "\n", ##__VA_ARGS__)
#define LOG_WARN(format, ...) printf("[WARNING]: " format "\n", ##__VA_ARGS__)
#define LOG_INFO(format, ...) printf("[INFO]: " format "\n", ##__VA_ARGS__)
#define LOG_ERROR(format, ...) printf("[ERROR]: " format "\n", ##__VA_ARGS__)
#define LOG_FATAL(format, ...) printf("[FATAL]: " format "\n", ##__VA_ARGS__)
#endif

2
include/ur_modern_driver/packet.h
View File

@@ -3,5 +3,5 @@
class Packet {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool parse_with(BinParser& bp) = 0;
};

2
include/ur_modern_driver/parser.h
View File

@@ -4,5 +4,5 @@
class Parser {
public:
virtual std::unique_ptr<Packet> parse(BinParser &bp) = 0;
virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
};

80
include/ur_modern_driver/pipeline.h
View File

@@ -1,21 +1,20 @@
#pragma once
#include <thread>
#include <atomic>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <thread>
#include <vector>
using namespace moodycamel;
using namespace moodycamel;
using namespace std;
template <typename T>
class IConsumer {
public:
virtual void setup_consumer() { }
virtual void teardown_consumer() { }
virtual void stop_consumer() { }
virtual void setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
virtual bool consume(unique_ptr<T> product) = 0;
};
@@ -23,33 +22,33 @@ public:
template <typename T>
class IProducer {
public:
virtual void setup_producer() { }
virtual void teardown_producer() { }
virtual void stop_producer() { }
virtual void setup_producer() {}
virtual void teardown_producer() {}
virtual void stop_producer() {}
virtual bool try_get(std::vector<unique_ptr<T>> &products) = 0;
virtual bool try_get(std::vector<unique_ptr<T> >& products) = 0;
};
template <typename T>
class Pipeline {
private:
IProducer<T> &_producer;
IConsumer<T> &_consumer;
BlockingReaderWriterQueue<unique_ptr<T>> _queue;
IProducer<T>& _producer;
IConsumer<T>& _consumer;
BlockingReaderWriterQueue<unique_ptr<T> > _queue;
atomic<bool> _running;
thread _pThread, _cThread;
void run_producer() {
void run_producer()
{
_producer.setup_producer();
std::vector<unique_ptr<T>> products;
while(_running) {
if(!_producer.try_get(products)) {
std::vector<unique_ptr<T> > products;
while (_running) {
if (!_producer.try_get(products)) {
break;
}
for(auto &p : products) {
if(!_queue.try_enqueue(std::move(p))) {
for (auto& p : products) {
if (!_queue.try_enqueue(std::move(p))) {
LOG_WARN("Pipeline owerflowed!");
}
}
@@ -60,27 +59,31 @@ private:
//todo cleanup
}
void run_consumer() {
void run_consumer()
{
_consumer.setup_consumer();
unique_ptr<T> product;
while(_running) {
while (_running) {
_queue.wait_dequeue(product);
if(!_consumer.consume(std::move(product)))
if (!_consumer.consume(std::move(product)))
break;
}
_consumer.teardown_consumer();
//todo cleanup
}
public:
Pipeline(IProducer<T> &producer, IConsumer<T> &consumer)
: _producer(producer),
_consumer(consumer),
_queue{32},
_running{false}
{ }
void run() {
if(_running)
public:
Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
: _producer(producer)
, _consumer(consumer)
, _queue{ 32 }
, _running{ false }
{
}
void run()
{
if (_running)
return;
_running = true;
@@ -88,10 +91,11 @@ public:
_cThread = thread(&Pipeline::run_consumer, this);
}
void stop() {
if(!_running)
void stop()
{
if (!_running)
return;
_consumer.stop_consumer();
_producer.stop_producer();

922
include/ur_modern_driver/queue/atomicops.h
View File

File diff suppressed because it is too large Load Diff

1278
include/ur_modern_driver/queue/readerwriterqueue.h
View File

File diff suppressed because it is too large Load Diff

282
include/ur_modern_driver/robot_state.h
View File

@@ -19,200 +19,202 @@
#ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_
#include <condition_variable>
#include <inttypes.h>
#include <vector>
#include <mutex>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <condition_variable>
#include <netinet/in.h>
#include <vector>
namespace message_types {
enum message_type {
ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
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
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
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
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
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];
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;
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;
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 {
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_ */

156
include/ur_modern_driver/robot_state_RT.h
View File

@@ -19,98 +19,98 @@
#ifndef ROBOT_STATE_RT_H_
#define ROBOT_STATE_RT_H_
#include <condition_variable>
#include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <netinet/in.h>
#include <condition_variable>
#include <stdlib.h>
#include <string.h>
#include <vector>
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
include/ur_modern_driver/ros/converter.h
View File

@@ -1,2 +1 @@
#pragma once

44
include/ur_modern_driver/ros/rt_publisher.h
View File

@@ -1,12 +1,12 @@
#pragma once
#include <cstdlib>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <vector>
#include "ur_modern_driver/ur/consumer.h"
@@ -26,34 +26,34 @@ private:
NodeHandle _nh;
Publisher _joint_pub;
Publisher _wrench_pub;
Publisher _tool_vel_pub;
Publisher _tool_vel_pub;
std::vector<std::string> _joint_names;
std::string _base_frame;
bool publish_joints(RTShared &packet, ros::Time &t);
bool publish_wrench(RTShared &packet, ros::Time &t);
bool publish_tool(RTShared &packet, ros::Time &t);
bool publish_joints(RTShared& packet, ros::Time& t);
bool publish_wrench(RTShared& packet, ros::Time& t);
bool publish_tool(RTShared& packet, ros::Time& t);
bool publish(RTShared &packet);
bool publish(RTShared& packet);
public:
RTPublisher(std::string &joint_prefix, std::string &base_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)),
_base_frame(base_frame)
RTPublisher(std::string& joint_prefix, std::string& base_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))
, _base_frame(base_frame)
{
for(auto const& j : JOINTS) {
for (auto const& j : JOINTS) {
_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 setup_consumer() {}
virtual void teardown_consumer() {}
virtual void stop_consumer() {}
};

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

@@ -1,46 +1,47 @@
#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/state.h"
class URRTPacketConsumer : public IConsumer<RTPacket> {
public:
virtual bool consume(unique_ptr<RTPacket> packet) {
virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(RTState_V1_6__7 &state) = 0;
virtual bool consume(RTState_V1_8 &state) = 0;
virtual bool consume(RTState_V3_0__1 &state) = 0;
virtual bool consume(RTState_V3_2__3 &state) = 0;
virtual bool consume(RTState_V1_6__7& state) = 0;
virtual bool consume(RTState_V1_8& state) = 0;
virtual bool consume(RTState_V3_0__1& state) = 0;
virtual bool consume(RTState_V3_2__3& state) = 0;
};
class URStatePacketConsumer : public IConsumer<StatePacket> {
public:
virtual bool consume(unique_ptr<StatePacket> packet) {
virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(MasterBoardData_V1_X &data) = 0;
virtual bool consume(MasterBoardData_V3_0__1 &data) = 0;
virtual bool consume(MasterBoardData_V3_2 &data) = 0;
virtual bool consume(RobotModeData_V1_X &data) = 0;
virtual bool consume(RobotModeData_V3_0__1 &data) = 0;
virtual bool consume(RobotModeData_V3_2 &data) = 0;
};
virtual bool consume(MasterBoardData_V1_X& data) = 0;
virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
virtual bool consume(MasterBoardData_V3_2& data) = 0;
virtual bool consume(RobotModeData_V1_X& data) = 0;
virtual bool consume(RobotModeData_V3_0__1& data) = 0;
virtual bool consume(RobotModeData_V3_2& data) = 0;
};
class URMessagePacketConsumer : public IConsumer<MessagePacket> {
public:
virtual bool consume(unique_ptr<MessagePacket> packet) {
virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this);
}
virtual bool consume(VersionMessage &message) = 0;
virtual bool consume(VersionMessage& message) = 0;
};

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

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

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

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

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

@@ -1,30 +1,33 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h"
enum class robot_message_type : uint8_t {
ROBOT_MESSAGE_TEXT = 0,
ROBOT_MESSAGE_PROGRAM_LABEL = 1,
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
ROBOT_MESSAGE_VERSION = 3,
ROBOT_MESSAGE_SAFETY_MODE = 5,
ROBOT_MESSAGE_ERROR_CODE = 6,
ROBOT_MESSAGE_KEY = 7,
ROBOT_MESSAGE_REQUEST_VALUE = 9,
ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
ROBOT_MESSAGE_TEXT = 0,
ROBOT_MESSAGE_PROGRAM_LABEL = 1,
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
ROBOT_MESSAGE_VERSION = 3,
ROBOT_MESSAGE_SAFETY_MODE = 5,
ROBOT_MESSAGE_ERROR_CODE = 6,
ROBOT_MESSAGE_KEY = 7,
ROBOT_MESSAGE_REQUEST_VALUE = 9,
ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
};
class URMessagePacketConsumer;
class MessagePacket {
public:
MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) { }
virtual bool parse_with(BinParser &bp) = 0;
virtual bool consume_with(URMessagePacketConsumer &consumer) = 0;
MessagePacket(uint64_t timestamp, uint8_t source)
: timestamp(timestamp)
, source(source)
{
}
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URMessagePacketConsumer& consumer) = 0;
uint64_t timestamp;
uint8_t source;
@@ -32,10 +35,13 @@ public:
class VersionMessage : public MessagePacket {
public:
VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { }
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URMessagePacketConsumer &consumer);
VersionMessage(uint64_t timestamp, uint8_t source)
: MessagePacket(timestamp, source)
{
}
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URMessagePacketConsumer& consumer);
std::string project_name;
uint8_t major_version;

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

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

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

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

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

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

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

@@ -1,14 +1,14 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/ur/state.h"
class SharedRobotModeData {
public:
virtual bool parse_with(BinParser &bp);
virtual bool parse_with(BinParser& bp);
uint64_t timestamp;
bool physical_robot_connected;
@@ -23,31 +23,30 @@ public:
enum class robot_mode_V1_X : uint8_t {
ROBOT_RUNNING_MODE = 0,
ROBOT_FREEDRIVE_MODE = 1,
ROBOT_READY_MODE = 2,
ROBOT_INITIALIZING_MODE = 3,
ROBOT_SECURITY_STOPPED_MODE = 4,
ROBOT_EMERGENCY_STOPPED_MODE = 5,
ROBOT_FATAL_ERROR_MODE = 6,
ROBOT_NO_POWER_MODE = 7,
ROBOT_NOT_CONNECTED_MODE = 8,
ROBOT_SHUTDOWN_MODE = 9,
ROBOT_SAFEGUARD_STOP_MODE = 10
ROBOT_FREEDRIVE_MODE = 1,
ROBOT_READY_MODE = 2,
ROBOT_INITIALIZING_MODE = 3,
ROBOT_SECURITY_STOPPED_MODE = 4,
ROBOT_EMERGENCY_STOPPED_MODE = 5,
ROBOT_FATAL_ERROR_MODE = 6,
ROBOT_NO_POWER_MODE = 7,
ROBOT_NOT_CONNECTED_MODE = 8,
ROBOT_SHUTDOWN_MODE = 9,
ROBOT_SAFEGUARD_STOP_MODE = 10
};
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
bool security_stopped;
robot_mode_V1_X robot_mode;
double speed_fraction;
static const size_t SIZE = SharedRobotModeData::SIZE
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V1_X)
+ sizeof(robot_mode_V1_X)
+ sizeof(double);
static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
@@ -55,14 +54,14 @@ public:
enum class robot_mode_V3_X : uint8_t {
DISCONNECTED = 0,
CONFIRM_SAFETY = 1,
BOOTING = 2,
POWER_OFF = 3,
POWER_ON = 4,
IDLE = 5,
BACKDRIVE = 6,
RUNNING = 7,
UPDATING_FIRMWARE = 8
CONFIRM_SAFETY = 1,
BOOTING = 2,
POWER_OFF = 3,
POWER_ON = 4,
IDLE = 5,
BACKDRIVE = 6,
RUNNING = 7,
UPDATING_FIRMWARE = 8
};
enum class robot_control_mode_V3_X : uint8_t {
@@ -74,9 +73,8 @@ enum class robot_control_mode_V3_X : uint8_t {
class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
bool protective_stopped;
@@ -85,10 +83,10 @@ public:
double target_speed_fraction;
double speed_scaling;
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V3_X)
static const size_t SIZE = SharedRobotModeData::SIZE
+ sizeof(uint8_t)
+ sizeof(robot_mode_V3_X)
+ sizeof(robot_control_mode_V3_X)
+ sizeof(double)
+ sizeof(double);
@@ -98,14 +96,13 @@ public:
class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
public:
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URStatePacketConsumer &consumer);
virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer& consumer);
double target_speed_fraction_limit;
static const size_t SIZE = RobotModeData_V3_0__1::SIZE
+ sizeof(double);
static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");
};

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

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

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

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

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

@@ -1,27 +1,27 @@
#pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
enum class package_type : uint8_t {
ROBOT_MODE_DATA = 0,
JOINT_DATA = 1,
TOOL_DATA = 2,
MASTERBOARD_DATA = 3,
CARTESIAN_INFO = 4,
KINEMATICS_INFO = 5,
CONFIGURATION_DATA = 6,
FORCE_MODE_DATA = 7,
ADDITIONAL_INFO = 8,
CALIBRATION_DATA = 9
ROBOT_MODE_DATA = 0,
JOINT_DATA = 1,
TOOL_DATA = 2,
MASTERBOARD_DATA = 3,
CARTESIAN_INFO = 4,
KINEMATICS_INFO = 5,
CONFIGURATION_DATA = 6,
FORCE_MODE_DATA = 7,
ADDITIONAL_INFO = 8,
CALIBRATION_DATA = 9
};
enum class message_type : uint8_t {
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
ROBOT_STATE = 16,
ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25
};
@@ -29,6 +29,6 @@ class URStatePacketConsumer;
class StatePacket {
public:
virtual bool parse_with(BinParser &bp) = 0;
virtual bool consume_with(URStatePacketConsumer &consumer) = 0;
virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URStatePacketConsumer& consumer) = 0;
};

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

@@ -1,73 +1,74 @@
#pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
#include <vector>
template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> {
private:
StatePacket* from_type(package_type type) {
switch(type) {
case package_type::ROBOT_MODE_DATA:
return new RMD;
case package_type::MASTERBOARD_DATA:
return new MBD;
default:
return nullptr;
StatePacket* from_type(package_type type)
{
switch (type) {
case package_type::ROBOT_MODE_DATA:
return new RMD;
case package_type::MASTERBOARD_DATA:
return new MBD;
default:
return nullptr;
}
}
public:
bool parse(BinParser &bp, std::vector<std::unique_ptr<StatePacket>> &results) {
bool parse(BinParser& bp, std::vector<std::unique_ptr<StatePacket> >& results)
{
int32_t packet_size;
message_type type;
bp.parse(packet_size);
bp.parse(type);
if(type != message_type::ROBOT_STATE) {
if (type != message_type::ROBOT_STATE) {
LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false;
}
while(!bp.empty()) {
if(!bp.check_size(sizeof(uint32_t))){
while (!bp.empty()) {
if (!bp.check_size(sizeof(uint32_t))) {
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if(!bp.check_size(static_cast<size_t>(sub_size))) {
if (!bp.check_size(static_cast<size_t>(sub_size))) {
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
//deconstruction of a sub parser will increment the position of the parent parser
//deconstruction of a sub parser will increment the position of the parent parser
BinParser sbp(bp, sub_size);
sbp.consume(sizeof(sub_size));
package_type type;
sbp.parse(type);
std::unique_ptr<StatePacket> packet(from_type(type));
if(packet == nullptr) {
if (packet == nullptr) {
sbp.consume();
LOG_INFO("Skipping sub-packet of type %d", type);
continue;
}
if(!packet->parse_with(sbp)) {
if (!packet->parse_with(sbp)) {
LOG_ERROR("Sub-package parsing of type %d failed!", type);
return false;
}
results.push_back(std::move(packet));
if(!sbp.empty()) {
if (!sbp.empty()) {
LOG_ERROR("Sub-package was not parsed completely!");
return false;
}
@@ -77,6 +78,6 @@ public:
}
};
typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;
typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X;
typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2;

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

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

56
include/ur_modern_driver/ur_communication.h
View File

@@ -19,46 +19,44 @@
#ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_
#include "robot_state.h"
#include "do_output.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include "robot_state.h"
#include <chrono>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <fcntl.h>
#include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
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_;
UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start();
void halt();
bool connected_;
RobotState* robot_state_;
UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start();
void halt();
};
#endif /* UR_COMMUNICATION_H_ */

118
include/ur_modern_driver/ur_driver.h
View File

@@ -19,83 +19,81 @@
#ifndef UR_DRIVER_H_
#define UR_DRIVER_H_
#include <mutex>
#include <condition_variable>
#include "ur_realtime_communication.h"
#include "ur_communication.h"
#include "do_output.h"
#include <vector>
#include "ur_communication.h"
#include "ur_realtime_communication.h"
#include <condition_variable>
#include <math.h>
#include <string>
#include <sys/types.h>
#include <sys/socket.h>
#include <mutex>
#include <netinet/in.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>
#include <chrono>
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);
void setMinPayload(double m);
void setMaxPayload(double m);
void setServojTime(double t);
void setServojLookahead(double t);
void setServojGain(double g);
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);
};
#endif /* UR_DRIVER_H_ */

97
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -58,16 +58,16 @@
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/robot_hw.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <math.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>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <math.h>
#include <ros/ros.h>
namespace ros_control_ur {
@@ -76,64 +76,61 @@ 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);
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_;
// Robot API
UrDriver* robot_;
double max_vel_change_;
// Robot API
UrDriver* robot_;
};
// class
}// namespace
} // namespace
#endif

80
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -19,58 +19,56 @@
#ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_
#include "robot_state_RT.h"
#include "do_output.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.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>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
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_;
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();
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();
};
#endif /* UR_REALTIME_COMMUNICATION_H_ */