Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00: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

95
.clang-format Normal file
View File

@@ -0,0 +1,95 @@
---
Language: Cpp
# BasedOnStyle: WebKit
AccessModifierOffset: -4
AlignAfterOpenBracket: DontAlign
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlinesLeft: false
AlignOperands: false
AlignTrailingComments: false
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: false
AlwaysBreakTemplateDeclarations: false
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: false
AfterControlStatement: false
AfterEnum: false
AfterFunction: true
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: false
AfterUnion: false
BeforeCatch: false
BeforeElse: false
IndentBraces: false
BreakBeforeBinaryOperators: All
BreakBeforeBraces: WebKit
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: true
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 0
CommentPragmas: '^ IWYU pragma:'
ConstructorInitializerAllOnOneLineOrOnePerLine: false
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: false
DerivePointerAlignment: false
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
IncludeCategories:
- Regex: '^"(llvm|llvm-c|clang|clang-c)/'
Priority: 2
- Regex: '^(<|"(gtest|isl|json)/)'
Priority: 3
- Regex: '.*'
Priority: 1
IncludeIsMainRegex: '$'
IndentCaseLabels: false
IndentWidth: 4
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: true
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
NamespaceIndentation: Inner
ObjCBlockIndentWidth: 4
ObjCSpaceAfterProperty: true
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 60
PointerAlignment: Left
ReflowComments: true
SortIncludes: true
SpaceAfterCStyleCast: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 1
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Cpp03
TabWidth: 8
UseTab: Never
...

127
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(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() {
~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
// 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;
}
// 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) {
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;
};

74
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 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,8 +91,9 @@ public:
_cThread = thread(&Pipeline::run_consumer, this);
}
void stop() {
if(!_running)
void stop()
{
if (!_running)
return;
_consumer.stop_consumer();

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

File diff suppressed because it is too large Load Diff

1220
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

42
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"
@@ -30,30 +30,30 @@ private:
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() {}
};

37
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(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> {
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;
};

64
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);
@@ -30,27 +30,29 @@ private:
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) {
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);
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);
return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V3_2__3);
}
}
};

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

@@ -1,15 +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 SharedMasterBoardData {
public:
virtual bool parse_with(BinParser &bp);
virtual bool parse_with(BinParser& bp);
int8_t analog_input_range0;
int8_t analog_input_range1;
@@ -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;

42
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) { }
VersionMessage(uint64_t timestamp, uint8_t source)
: MessagePacket(timestamp, source)
{
}
virtual bool parse_with(BinParser &bp);
virtual bool consume_with(URMessagePacketConsumer &consumer);
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;
};

28
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) { }
URProducer(URStream& stream, URParser<T>& parser)
: _stream(stream)
, _parser(parser)
{
}
void setup_producer() {
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;
}

59
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,23 +23,22 @@ 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;
@@ -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;
@@ -98,9 +96,8 @@ 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;

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

@@ -1,19 +1,19 @@
#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;
}
@@ -21,7 +21,7 @@ public:
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));

34
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
+ 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,7 +94,6 @@ public:
double i_robot;
double v_actual[6];
static const size_t SIZE = RTShared::SIZE
+ sizeof(double[6]) * 3
+ sizeof(double3_t)
@@ -109,8 +105,8 @@ 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;

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

@@ -1,26 +1,26 @@
#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_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;
};

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

@@ -1,47 +1,48 @@
#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;
}
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;
}
@@ -54,20 +55,20 @@ public:
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;

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

@@ -1,9 +1,9 @@
#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
class URStream {
@@ -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(std::string& host, int port)
: _host(host)
, _port(port)
, _initialized(false)
, _stopping(false)
{
}
~URStream() {
~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_ */

39
src/do_output.cpp
View File

@@ -18,40 +18,45 @@
#include "ur_modern_driver/do_output.h"
void print_debug(std::string inp) {
void print_debug(std::string inp)
{
#ifdef ROS_BUILD
ROS_DEBUG("%s", inp.c_str());
ROS_DEBUG("%s", inp.c_str());
#else
printf("DEBUG: %s\n", inp.c_str());
printf("DEBUG: %s\n", inp.c_str());
#endif
}
void print_info(std::string inp) {
void print_info(std::string inp)
{
#ifdef ROS_BUILD
ROS_INFO("%s", inp.c_str());
ROS_INFO("%s", inp.c_str());
#else
printf("INFO: %s\n", inp.c_str());
printf("INFO: %s\n", inp.c_str());
#endif
}
void print_warning(std::string inp) {
void print_warning(std::string inp)
{
#ifdef ROS_BUILD
ROS_WARN("%s", inp.c_str());
ROS_WARN("%s", inp.c_str());
#else
printf("WARNING: %s\n", inp.c_str());
printf("WARNING: %s\n", inp.c_str());
#endif
}
void print_error(std::string inp) {
void print_error(std::string inp)
{
#ifdef ROS_BUILD
ROS_ERROR("%s", inp.c_str());
ROS_ERROR("%s", inp.c_str());
#else
printf("ERROR: %s\n", inp.c_str());
printf("ERROR: %s\n", inp.c_str());
#endif
}
void print_fatal(std::string inp) {
void print_fatal(std::string inp)
{
#ifdef ROS_BUILD
ROS_FATAL("%s", inp.c_str());
ros::shutdown();
ROS_FATAL("%s", inp.c_str());
ros::shutdown();
#else
printf("FATAL: %s\n", inp.c_str());
exit(1);
printf("FATAL: %s\n", inp.c_str());
exit(1);
#endif
}

671
src/robot_state.cpp
View File

@@ -18,370 +18,391 @@
#include "ur_modern_driver/robot_state.h"
RobotState::RobotState(std::condition_variable& msg_cond) {
version_msg_.major_version = 0;
version_msg_.minor_version = 0;
new_data_available_ = false;
pMsg_cond_ = &msg_cond;
RobotState::setDisconnected();
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
RobotState::RobotState(std::condition_variable& msg_cond)
{
version_msg_.major_version = 0;
version_msg_.minor_version = 0;
new_data_available_ = false;
pMsg_cond_ = &msg_cond;
RobotState::setDisconnected();
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
}
double RobotState::ntohd(uint64_t nf) {
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
double RobotState::ntohd(uint64_t nf)
{
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
}
void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0;
while (buf_length > offset) {
int len;
unsigned char message_type;
memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len);
if (len + offset > buf_length) {
return;
}
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
switch (message_type) {
case messageType::ROBOT_MESSAGE:
RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
break;
case messageType::ROBOT_STATE:
RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
break;
case messageType::PROGRAM_STATE_MESSAGE:
//Don't do anything atm...
default:
break;
}
offset += len;
}
return;
void RobotState::unpack(uint8_t* buf, unsigned int buf_length)
{
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0;
while (buf_length > offset) {
int len;
unsigned char message_type;
memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len);
if (len + offset > buf_length) {
return;
}
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
switch (message_type) {
case messageType::ROBOT_MESSAGE:
RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
break;
case messageType::ROBOT_STATE:
RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
break;
case messageType::PROGRAM_STATE_MESSAGE:
//Don't do anything atm...
default:
break;
}
offset += len;
}
return;
}
void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
uint32_t len) {
offset += 5;
uint64_t timestamp;
int8_t source, robot_message_type;
memcpy(&timestamp, &buf[offset], sizeof(timestamp));
offset += sizeof(timestamp);
memcpy(&source, &buf[offset], sizeof(source));
offset += sizeof(source);
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
offset += sizeof(robot_message_type);
switch (robot_message_type) {
case robotMessageType::ROBOT_MESSAGE_VERSION:
val_lock_.lock();
version_msg_.timestamp = timestamp;
version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len);
val_lock_.unlock();
break;
default:
break;
}
void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
uint32_t len)
{
offset += 5;
uint64_t timestamp;
int8_t source, robot_message_type;
memcpy(&timestamp, &buf[offset], sizeof(timestamp));
offset += sizeof(timestamp);
memcpy(&source, &buf[offset], sizeof(source));
offset += sizeof(source);
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
offset += sizeof(robot_message_type);
switch (robot_message_type) {
case robotMessageType::ROBOT_MESSAGE_VERSION:
val_lock_.lock();
version_msg_.timestamp = timestamp;
version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len);
val_lock_.unlock();
break;
default:
break;
}
}
void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
uint32_t len) {
offset += 5;
while (offset < len) {
int32_t length;
uint8_t package_type;
memcpy(&length, &buf[offset], sizeof(length));
length = ntohl(length);
memcpy(&package_type, &buf[offset + sizeof(length)],
sizeof(package_type));
switch (package_type) {
case packageType::ROBOT_MODE_DATA:
val_lock_.lock();
RobotState::unpackRobotMode(buf, offset + 5);
val_lock_.unlock();
break;
case packageType::MASTERBOARD_DATA:
val_lock_.lock();
RobotState::unpackRobotStateMasterboard(buf, offset + 5);
val_lock_.unlock();
break;
default:
break;
}
offset += length;
}
new_data_available_ = true;
pMsg_cond_->notify_all();
void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
uint32_t len)
{
offset += 5;
while (offset < len) {
int32_t length;
uint8_t package_type;
memcpy(&length, &buf[offset], sizeof(length));
length = ntohl(length);
memcpy(&package_type, &buf[offset + sizeof(length)],
sizeof(package_type));
switch (package_type) {
case packageType::ROBOT_MODE_DATA:
val_lock_.lock();
RobotState::unpackRobotMode(buf, offset + 5);
val_lock_.unlock();
break;
case packageType::MASTERBOARD_DATA:
val_lock_.lock();
RobotState::unpackRobotStateMasterboard(buf, offset + 5);
val_lock_.unlock();
break;
default:
break;
}
offset += length;
}
new_data_available_ = true;
pMsg_cond_->notify_all();
}
void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
uint32_t len) {
memcpy(&version_msg_.project_name_size, &buf[offset],
sizeof(version_msg_.project_name_size));
offset += sizeof(version_msg_.project_name_size);
memcpy(&version_msg_.project_name, &buf[offset],
sizeof(char) * version_msg_.project_name_size);
offset += version_msg_.project_name_size;
version_msg_.project_name[version_msg_.project_name_size] = '\0';
memcpy(&version_msg_.major_version, &buf[offset],
sizeof(version_msg_.major_version));
offset += sizeof(version_msg_.major_version);
memcpy(&version_msg_.minor_version, &buf[offset],
sizeof(version_msg_.minor_version));
offset += sizeof(version_msg_.minor_version);
memcpy(&version_msg_.svn_revision, &buf[offset],
sizeof(version_msg_.svn_revision));
offset += sizeof(version_msg_.svn_revision);
version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
version_msg_.build_date[len - offset] = '\0';
if (version_msg_.major_version < 2) {
robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
}
void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
uint32_t len)
{
memcpy(&version_msg_.project_name_size, &buf[offset],
sizeof(version_msg_.project_name_size));
offset += sizeof(version_msg_.project_name_size);
memcpy(&version_msg_.project_name, &buf[offset],
sizeof(char) * version_msg_.project_name_size);
offset += version_msg_.project_name_size;
version_msg_.project_name[version_msg_.project_name_size] = '\0';
memcpy(&version_msg_.major_version, &buf[offset],
sizeof(version_msg_.major_version));
offset += sizeof(version_msg_.major_version);
memcpy(&version_msg_.minor_version, &buf[offset],
sizeof(version_msg_.minor_version));
offset += sizeof(version_msg_.minor_version);
memcpy(&version_msg_.svn_revision, &buf[offset],
sizeof(version_msg_.svn_revision));
offset += sizeof(version_msg_.svn_revision);
version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
version_msg_.build_date[len - offset] = '\0';
if (version_msg_.major_version < 2) {
robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
}
}
void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
offset += sizeof(robot_mode_.timestamp);
uint8_t tmp;
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isRobotConnected = true;
else
robot_mode_.isRobotConnected = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isRealRobotEnabled = true;
else
robot_mode_.isRealRobotEnabled = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
//printf("PowerOnRobot: %d\n", tmp);
if (tmp > 0)
robot_mode_.isPowerOnRobot = true;
else
robot_mode_.isPowerOnRobot = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isEmergencyStopped = true;
else
robot_mode_.isEmergencyStopped = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProtectiveStopped = true;
else
robot_mode_.isProtectiveStopped = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProgramRunning = true;
else
robot_mode_.isProgramRunning = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProgramPaused = true;
else
robot_mode_.isProgramPaused = false;
offset += sizeof(tmp);
memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
offset += sizeof(robot_mode_.robotMode);
uint64_t temp;
if (RobotState::getVersion() > 2.) {
memcpy(&robot_mode_.controlMode, &buf[offset],
sizeof(robot_mode_.controlMode));
offset += sizeof(robot_mode_.controlMode);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
}
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
robot_mode_.speedScaling = RobotState::ntohd(temp);
void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
{
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
offset += sizeof(robot_mode_.timestamp);
uint8_t tmp;
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isRobotConnected = true;
else
robot_mode_.isRobotConnected = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isRealRobotEnabled = true;
else
robot_mode_.isRealRobotEnabled = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
//printf("PowerOnRobot: %d\n", tmp);
if (tmp > 0)
robot_mode_.isPowerOnRobot = true;
else
robot_mode_.isPowerOnRobot = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isEmergencyStopped = true;
else
robot_mode_.isEmergencyStopped = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProtectiveStopped = true;
else
robot_mode_.isProtectiveStopped = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProgramRunning = true;
else
robot_mode_.isProgramRunning = false;
offset += sizeof(tmp);
memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0)
robot_mode_.isProgramPaused = true;
else
robot_mode_.isProgramPaused = false;
offset += sizeof(tmp);
memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
offset += sizeof(robot_mode_.robotMode);
uint64_t temp;
if (RobotState::getVersion() > 2.) {
memcpy(&robot_mode_.controlMode, &buf[offset],
sizeof(robot_mode_.controlMode));
offset += sizeof(robot_mode_.controlMode);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
}
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
robot_mode_.speedScaling = RobotState::ntohd(temp);
}
void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
unsigned int offset) {
if (RobotState::getVersion() < 3.0) {
int16_t digital_input_bits, digital_output_bits;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
offset += sizeof(digital_input_bits);
memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
offset += sizeof(digital_output_bits);
mb_data_.digitalInputBits = ntohs(digital_input_bits);
mb_data_.digitalOutputBits = ntohs(digital_output_bits);
} else {
memcpy(&mb_data_.digitalInputBits, &buf[offset],
sizeof(mb_data_.digitalInputBits));
offset += sizeof(mb_data_.digitalInputBits);
mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
memcpy(&mb_data_.digitalOutputBits, &buf[offset],
sizeof(mb_data_.digitalOutputBits));
offset += sizeof(mb_data_.digitalOutputBits);
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
}
void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
unsigned int offset)
{
if (RobotState::getVersion() < 3.0) {
int16_t digital_input_bits, digital_output_bits;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
offset += sizeof(digital_input_bits);
memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
offset += sizeof(digital_output_bits);
mb_data_.digitalInputBits = ntohs(digital_input_bits);
mb_data_.digitalOutputBits = ntohs(digital_output_bits);
} else {
memcpy(&mb_data_.digitalInputBits, &buf[offset],
sizeof(mb_data_.digitalInputBits));
offset += sizeof(mb_data_.digitalInputBits);
mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
memcpy(&mb_data_.digitalOutputBits, &buf[offset],
sizeof(mb_data_.digitalOutputBits));
offset += sizeof(mb_data_.digitalOutputBits);
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
}
memcpy(&mb_data_.analogInputRange0, &buf[offset],
sizeof(mb_data_.analogInputRange0));
offset += sizeof(mb_data_.analogInputRange0);
memcpy(&mb_data_.analogInputRange1, &buf[offset],
sizeof(mb_data_.analogInputRange1));
offset += sizeof(mb_data_.analogInputRange1);
uint64_t temp;
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogInput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogInput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
sizeof(mb_data_.analogOutputDomain0));
offset += sizeof(mb_data_.analogOutputDomain0);
memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
sizeof(mb_data_.analogOutputDomain1));
offset += sizeof(mb_data_.analogOutputDomain1);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogOutput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogOutput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.analogInputRange0, &buf[offset],
sizeof(mb_data_.analogInputRange0));
offset += sizeof(mb_data_.analogInputRange0);
memcpy(&mb_data_.analogInputRange1, &buf[offset],
sizeof(mb_data_.analogInputRange1));
offset += sizeof(mb_data_.analogInputRange1);
uint64_t temp;
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogInput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogInput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
sizeof(mb_data_.analogOutputDomain0));
offset += sizeof(mb_data_.analogOutputDomain0);
memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
sizeof(mb_data_.analogOutputDomain1));
offset += sizeof(mb_data_.analogOutputDomain1);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogOutput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp);
mb_data_.analogOutput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
sizeof(mb_data_.masterBoardTemperature));
offset += sizeof(mb_data_.masterBoardTemperature);
mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
memcpy(&mb_data_.robotVoltage48V, &buf[offset],
sizeof(mb_data_.robotVoltage48V));
offset += sizeof(mb_data_.robotVoltage48V);
mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
offset += sizeof(mb_data_.robotCurrent);
mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
memcpy(&mb_data_.masterIOCurrent, &buf[offset],
sizeof(mb_data_.masterIOCurrent));
offset += sizeof(mb_data_.masterIOCurrent);
mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
sizeof(mb_data_.masterBoardTemperature));
offset += sizeof(mb_data_.masterBoardTemperature);
mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
memcpy(&mb_data_.robotVoltage48V, &buf[offset],
sizeof(mb_data_.robotVoltage48V));
offset += sizeof(mb_data_.robotVoltage48V);
mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
offset += sizeof(mb_data_.robotCurrent);
mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
memcpy(&mb_data_.masterIOCurrent, &buf[offset],
sizeof(mb_data_.masterIOCurrent));
offset += sizeof(mb_data_.masterIOCurrent);
mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
offset += sizeof(mb_data_.safetyMode);
memcpy(&mb_data_.masterOnOffState, &buf[offset],
sizeof(mb_data_.masterOnOffState));
offset += sizeof(mb_data_.masterOnOffState);
memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
offset += sizeof(mb_data_.safetyMode);
memcpy(&mb_data_.masterOnOffState, &buf[offset],
sizeof(mb_data_.masterOnOffState));
offset += sizeof(mb_data_.masterOnOffState);
memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
sizeof(mb_data_.euromap67InterfaceInstalled));
offset += sizeof(mb_data_.euromap67InterfaceInstalled);
if (mb_data_.euromap67InterfaceInstalled != 0) {
memcpy(&mb_data_.euromapInputBits, &buf[offset],
sizeof(mb_data_.euromapInputBits));
offset += sizeof(mb_data_.euromapInputBits);
mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
memcpy(&mb_data_.euromapOutputBits, &buf[offset],
sizeof(mb_data_.euromapOutputBits));
offset += sizeof(mb_data_.euromapOutputBits);
mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
if (RobotState::getVersion() < 3.0) {
int16_t euromap_voltage, euromap_current;
memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
offset += sizeof(euromap_voltage);
memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
offset += sizeof(euromap_current);
mb_data_.euromapVoltage = ntohs(euromap_voltage);
mb_data_.euromapCurrent = ntohs(euromap_current);
} else {
memcpy(&mb_data_.euromapVoltage, &buf[offset],
sizeof(mb_data_.euromapVoltage));
offset += sizeof(mb_data_.euromapVoltage);
mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
memcpy(&mb_data_.euromapCurrent, &buf[offset],
sizeof(mb_data_.euromapCurrent));
offset += sizeof(mb_data_.euromapCurrent);
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
}
}
memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
sizeof(mb_data_.euromap67InterfaceInstalled));
offset += sizeof(mb_data_.euromap67InterfaceInstalled);
if (mb_data_.euromap67InterfaceInstalled != 0) {
memcpy(&mb_data_.euromapInputBits, &buf[offset],
sizeof(mb_data_.euromapInputBits));
offset += sizeof(mb_data_.euromapInputBits);
mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
memcpy(&mb_data_.euromapOutputBits, &buf[offset],
sizeof(mb_data_.euromapOutputBits));
offset += sizeof(mb_data_.euromapOutputBits);
mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
if (RobotState::getVersion() < 3.0) {
int16_t euromap_voltage, euromap_current;
memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
offset += sizeof(euromap_voltage);
memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
offset += sizeof(euromap_current);
mb_data_.euromapVoltage = ntohs(euromap_voltage);
mb_data_.euromapCurrent = ntohs(euromap_current);
} else {
memcpy(&mb_data_.euromapVoltage, &buf[offset],
sizeof(mb_data_.euromapVoltage));
offset += sizeof(mb_data_.euromapVoltage);
mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
memcpy(&mb_data_.euromapCurrent, &buf[offset],
sizeof(mb_data_.euromapCurrent));
offset += sizeof(mb_data_.euromapCurrent);
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
}
}
}
double RobotState::getVersion() {
double ver;
val_lock_.lock();
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
+ .0000001 * version_msg_.svn_revision;
val_lock_.unlock();
return ver;
double RobotState::getVersion()
{
double ver;
val_lock_.lock();
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
+ .0000001 * version_msg_.svn_revision;
val_lock_.unlock();
return ver;
}
void RobotState::finishedReading() {
new_data_available_ = false;
void RobotState::finishedReading()
{
new_data_available_ = false;
}
bool RobotState::getNewDataAvailable() {
return new_data_available_;
bool RobotState::getNewDataAvailable()
{
return new_data_available_;
}
int RobotState::getDigitalInputBits() {
return mb_data_.digitalInputBits;
int RobotState::getDigitalInputBits()
{
return mb_data_.digitalInputBits;
}
int RobotState::getDigitalOutputBits() {
return mb_data_.digitalOutputBits;
int RobotState::getDigitalOutputBits()
{
return mb_data_.digitalOutputBits;
}
double RobotState::getAnalogInput0() {
return mb_data_.analogInput0;
double RobotState::getAnalogInput0()
{
return mb_data_.analogInput0;
}
double RobotState::getAnalogInput1() {
return mb_data_.analogInput1;
double RobotState::getAnalogInput1()
{
return mb_data_.analogInput1;
}
double RobotState::getAnalogOutput0() {
return mb_data_.analogOutput0;
double RobotState::getAnalogOutput0()
{
return mb_data_.analogOutput0;
}
double RobotState::getAnalogOutput1() {
return mb_data_.analogOutput1;
double RobotState::getAnalogOutput1()
{
return mb_data_.analogOutput1;
}
bool RobotState::isRobotConnected() {
return robot_mode_.isRobotConnected;
bool RobotState::isRobotConnected()
{
return robot_mode_.isRobotConnected;
}
bool RobotState::isRealRobotEnabled() {
return robot_mode_.isRealRobotEnabled;
bool RobotState::isRealRobotEnabled()
{
return robot_mode_.isRealRobotEnabled;
}
bool RobotState::isPowerOnRobot() {
return robot_mode_.isPowerOnRobot;
bool RobotState::isPowerOnRobot()
{
return robot_mode_.isPowerOnRobot;
}
bool RobotState::isEmergencyStopped() {
return robot_mode_.isEmergencyStopped;
bool RobotState::isEmergencyStopped()
{
return robot_mode_.isEmergencyStopped;
}
bool RobotState::isProtectiveStopped() {
return robot_mode_.isProtectiveStopped;
bool RobotState::isProtectiveStopped()
{
return robot_mode_.isProtectiveStopped;
}
bool RobotState::isProgramRunning() {
return robot_mode_.isProgramRunning;
bool RobotState::isProgramRunning()
{
return robot_mode_.isProgramRunning;
}
bool RobotState::isProgramPaused() {
return robot_mode_.isProgramPaused;
bool RobotState::isProgramPaused()
{
return robot_mode_.isProgramPaused;
}
unsigned char RobotState::getRobotMode() {
return robot_mode_.robotMode;
unsigned char RobotState::getRobotMode()
{
return robot_mode_.robotMode;
}
bool RobotState::isReady() {
if (robot_mode_.robotMode == robot_mode_running_) {
return true;
}
return false;
bool RobotState::isReady()
{
if (robot_mode_.robotMode == robot_mode_running_) {
return true;
}
return false;
}
void RobotState::setDisconnected() {
robot_mode_.isRobotConnected = false;
robot_mode_.isRealRobotEnabled = false;
robot_mode_.isPowerOnRobot = false;
void RobotState::setDisconnected()
{
robot_mode_.isRobotConnected = false;
robot_mode_.isRealRobotEnabled = false;
robot_mode_.isPowerOnRobot = false;
}

766
src/robot_state_RT.cpp
View File

@@ -18,420 +18,458 @@
#include "ur_modern_driver/robot_state_RT.h"
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
version_ = 0.0;
time_ = 0.0;
q_target_.assign(6, 0.0);
qd_target_.assign(6, 0.0);
qdd_target_.assign(6, 0.0);
i_target_.assign(6, 0.0);
m_target_.assign(6, 0.0);
q_actual_.assign(6, 0.0);
qd_actual_.assign(6, 0.0);
i_actual_.assign(6, 0.0);
i_control_.assign(6, 0.0);
tool_vector_actual_.assign(6, 0.0);
tcp_speed_actual_.assign(6, 0.0);
tcp_force_.assign(6, 0.0);
tool_vector_target_.assign(6, 0.0);
tcp_speed_target_.assign(6, 0.0);
digital_input_bits_.assign(64, false);
motor_temperatures_.assign(6, 0.0);
controller_timer_ = 0.0;
robot_mode_ = 0.0;
joint_modes_.assign(6, 0.0);
safety_mode_ = 0.0;
tool_accelerometer_values_.assign(3, 0.0);
speed_scaling_ = 0.0;
linear_momentum_norm_ = 0.0;
v_main_ = 0.0;
v_robot_ = 0.0;
i_robot_ = 0.0;
v_actual_.assign(6, 0.0);
data_published_ = false;
controller_updated_ = false;
pMsg_cond_ = &msg_cond;
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond)
{
version_ = 0.0;
time_ = 0.0;
q_target_.assign(6, 0.0);
qd_target_.assign(6, 0.0);
qdd_target_.assign(6, 0.0);
i_target_.assign(6, 0.0);
m_target_.assign(6, 0.0);
q_actual_.assign(6, 0.0);
qd_actual_.assign(6, 0.0);
i_actual_.assign(6, 0.0);
i_control_.assign(6, 0.0);
tool_vector_actual_.assign(6, 0.0);
tcp_speed_actual_.assign(6, 0.0);
tcp_force_.assign(6, 0.0);
tool_vector_target_.assign(6, 0.0);
tcp_speed_target_.assign(6, 0.0);
digital_input_bits_.assign(64, false);
motor_temperatures_.assign(6, 0.0);
controller_timer_ = 0.0;
robot_mode_ = 0.0;
joint_modes_.assign(6, 0.0);
safety_mode_ = 0.0;
tool_accelerometer_values_.assign(3, 0.0);
speed_scaling_ = 0.0;
linear_momentum_norm_ = 0.0;
v_main_ = 0.0;
v_robot_ = 0.0;
i_robot_ = 0.0;
v_actual_.assign(6, 0.0);
data_published_ = false;
controller_updated_ = false;
pMsg_cond_ = &msg_cond;
}
RobotStateRT::~RobotStateRT() {
/* Make sure nobody is waiting after this thread is destroyed */
data_published_ = true;
controller_updated_ = true;
pMsg_cond_->notify_all();
RobotStateRT::~RobotStateRT()
{
/* Make sure nobody is waiting after this thread is destroyed */
data_published_ = true;
controller_updated_ = true;
pMsg_cond_->notify_all();
}
void RobotStateRT::setDataPublished() {
data_published_ = false;
void RobotStateRT::setDataPublished()
{
data_published_ = false;
}
bool RobotStateRT::getDataPublished() {
return data_published_;
bool RobotStateRT::getDataPublished()
{
return data_published_;
}
void RobotStateRT::setControllerUpdated() {
controller_updated_ = false;
void RobotStateRT::setControllerUpdated()
{
controller_updated_ = false;
}
bool RobotStateRT::getControllerUpdated() {
return controller_updated_;
bool RobotStateRT::getControllerUpdated()
{
return controller_updated_;
}
double RobotStateRT::ntohd(uint64_t nf) {
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
double RobotStateRT::ntohd(uint64_t nf)
{
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
}
std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
int nr_of_vals) {
uint64_t q;
std::vector<double> ret;
for (int i = 0; i < nr_of_vals; i++) {
memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
ret.push_back(ntohd(q));
}
return ret;
std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index,
int nr_of_vals)
{
uint64_t q;
std::vector<double> ret;
for (int i = 0; i < nr_of_vals; i++) {
memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
ret.push_back(ntohd(q));
}
return ret;
}
std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
std::vector<bool> ret;
for (int i = 0; i < 64; i++) {
ret.push_back((data & (1 << i)) >> i);
}
return ret;
std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data)
{
std::vector<bool> ret;
for (int i = 0; i < 64; i++) {
ret.push_back((data & (1 << i)) >> i);
}
return ret;
}
void RobotStateRT::setVersion(double ver) {
val_lock_.lock();
version_ = ver;
val_lock_.unlock();
void RobotStateRT::setVersion(double ver)
{
val_lock_.lock();
version_ = ver;
val_lock_.unlock();
}
double RobotStateRT::getVersion() {
double ret;
val_lock_.lock();
ret = version_;
val_lock_.unlock();
return ret;
double RobotStateRT::getVersion()
{
double ret;
val_lock_.lock();
ret = version_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getTime() {
double ret;
val_lock_.lock();
ret = time_;
val_lock_.unlock();
return ret;
double RobotStateRT::getTime()
{
double ret;
val_lock_.lock();
ret = time_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = q_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getQTarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = q_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQdTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = qd_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getQdTarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = qd_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQddTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = qdd_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getQddTarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = qdd_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getITarget() {
std::vector<double> ret;
val_lock_.lock();
ret = i_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getITarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = i_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getMTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = m_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getMTarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = m_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQActual() {
std::vector<double> ret;
val_lock_.lock();
ret = q_actual_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getQActual()
{
std::vector<double> ret;
val_lock_.lock();
ret = q_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQdActual() {
std::vector<double> ret;
val_lock_.lock();
ret = qd_actual_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getQdActual()
{
std::vector<double> ret;
val_lock_.lock();
ret = qd_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getIActual() {
std::vector<double> ret;
val_lock_.lock();
ret = i_actual_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getIActual()
{
std::vector<double> ret;
val_lock_.lock();
ret = i_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getIControl() {
std::vector<double> ret;
val_lock_.lock();
ret = i_control_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getIControl()
{
std::vector<double> ret;
val_lock_.lock();
ret = i_control_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolVectorActual() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_actual_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getToolVectorActual()
{
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpSpeedActual() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_actual_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getTcpSpeedActual()
{
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpForce() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_force_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getTcpForce()
{
std::vector<double> ret;
val_lock_.lock();
ret = tcp_force_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolVectorTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getToolVectorTarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpSpeedTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_target_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getTcpSpeedTarget()
{
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_target_;
val_lock_.unlock();
return ret;
}
std::vector<bool> RobotStateRT::getDigitalInputBits() {
std::vector<bool> ret;
val_lock_.lock();
ret = digital_input_bits_;
val_lock_.unlock();
return ret;
std::vector<bool> RobotStateRT::getDigitalInputBits()
{
std::vector<bool> ret;
val_lock_.lock();
ret = digital_input_bits_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getMotorTemperatures() {
std::vector<double> ret;
val_lock_.lock();
ret = motor_temperatures_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getMotorTemperatures()
{
std::vector<double> ret;
val_lock_.lock();
ret = motor_temperatures_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getControllerTimer() {
double ret;
val_lock_.lock();
ret = controller_timer_;
val_lock_.unlock();
return ret;
double RobotStateRT::getControllerTimer()
{
double ret;
val_lock_.lock();
ret = controller_timer_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getRobotMode() {
double ret;
val_lock_.lock();
ret = robot_mode_;
val_lock_.unlock();
return ret;
double RobotStateRT::getRobotMode()
{
double ret;
val_lock_.lock();
ret = robot_mode_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getJointModes() {
std::vector<double> ret;
val_lock_.lock();
ret = joint_modes_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getJointModes()
{
std::vector<double> ret;
val_lock_.lock();
ret = joint_modes_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getSafety_mode() {
double ret;
val_lock_.lock();
ret = safety_mode_;
val_lock_.unlock();
return ret;
double RobotStateRT::getSafety_mode()
{
double ret;
val_lock_.lock();
ret = safety_mode_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolAccelerometerValues() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_accelerometer_values_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getToolAccelerometerValues()
{
std::vector<double> ret;
val_lock_.lock();
ret = tool_accelerometer_values_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getSpeedScaling() {
double ret;
val_lock_.lock();
ret = speed_scaling_;
val_lock_.unlock();
return ret;
double RobotStateRT::getSpeedScaling()
{
double ret;
val_lock_.lock();
ret = speed_scaling_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getLinearMomentumNorm() {
double ret;
val_lock_.lock();
ret = linear_momentum_norm_;
val_lock_.unlock();
return ret;
double RobotStateRT::getLinearMomentumNorm()
{
double ret;
val_lock_.lock();
ret = linear_momentum_norm_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getVMain() {
double ret;
val_lock_.lock();
ret = v_main_;
val_lock_.unlock();
return ret;
double RobotStateRT::getVMain()
{
double ret;
val_lock_.lock();
ret = v_main_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getVRobot() {
double ret;
val_lock_.lock();
ret = v_robot_;
val_lock_.unlock();
return ret;
double RobotStateRT::getVRobot()
{
double ret;
val_lock_.lock();
ret = v_robot_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getIRobot() {
double ret;
val_lock_.lock();
ret = i_robot_;
val_lock_.unlock();
return ret;
double RobotStateRT::getIRobot()
{
double ret;
val_lock_.lock();
ret = i_robot_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getVActual() {
std::vector<double> ret;
val_lock_.lock();
ret = v_actual_;
val_lock_.unlock();
return ret;
std::vector<double> RobotStateRT::getVActual()
{
std::vector<double> ret;
val_lock_.lock();
ret = v_actual_;
val_lock_.unlock();
return ret;
}
void RobotStateRT::unpack(uint8_t * buf) {
int64_t digital_input_bits;
uint64_t unpack_to;
uint16_t offset = 0;
val_lock_.lock();
int len;
memcpy(&len, &buf[offset], sizeof(len));
void RobotStateRT::unpack(uint8_t* buf)
{
int64_t digital_input_bits;
uint64_t unpack_to;
uint16_t offset = 0;
val_lock_.lock();
int len;
memcpy(&len, &buf[offset], sizeof(len));
offset += sizeof(len);
len = ntohl(len);
offset += sizeof(len);
len = ntohl(len);
//Check the correct message length is received
bool len_good = true;
if (version_ >= 1.6 && version_ < 1.7) { //v1.6
if (len != 756)
len_good = false;
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
if (len != 764)
len_good = false;
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
if (len != 812)
len_good = false;
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
if (len != 1044)
len_good = false;
} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
if (len != 1060)
len_good = false;
}
//Check the correct message length is received
bool len_good = true;
if (version_ >= 1.6 && version_ < 1.7) { //v1.6
if (len != 756)
len_good = false;
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
if (len != 764)
len_good = false;
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
if (len != 812)
len_good = false;
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
if (len != 1044)
len_good = false;
} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
if (len != 1060)
len_good = false;
}
if (!len_good) {
printf("Wrong length of message on RT interface: %i\n", len);
val_lock_.unlock();
return;
}
if (!len_good) {
printf("Wrong length of message on RT interface: %i\n", len);
val_lock_.unlock();
return;
}
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qdd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
m_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
q_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
if (version_ <= 1.9) {
if (version_ > 1.6)
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * (3 + 15);
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
} else {
i_control_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_target_ = unpackVector(buf, offset, 6);
}
offset += sizeof(double) * 6;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
offset += sizeof(double);
motor_temperatures_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
controller_timer_ = ntohd(unpack_to);
if (version_ > 1.6) {
offset += sizeof(double) * 2;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
robot_mode_ = ntohd(unpack_to);
if (version_ > 1.7) {
offset += sizeof(double);
joint_modes_ = unpackVector(buf, offset, 6);
}
}
if (version_ > 1.8) {
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
safety_mode_ = ntohd(unpack_to);
offset += sizeof(double);
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * 3;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
speed_scaling_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
linear_momentum_norm_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_main_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_robot_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
i_robot_ = ntohd(unpack_to);
offset += sizeof(double);
v_actual_ = unpackVector(buf, offset, 6);
}
val_lock_.unlock();
controller_updated_ = true;
data_published_ = true;
pMsg_cond_->notify_all();
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qdd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
m_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
q_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
if (version_ <= 1.9) {
if (version_ > 1.6)
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * (3 + 15);
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
} else {
i_control_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_target_ = unpackVector(buf, offset, 6);
}
offset += sizeof(double) * 6;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
offset += sizeof(double);
motor_temperatures_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
controller_timer_ = ntohd(unpack_to);
if (version_ > 1.6) {
offset += sizeof(double) * 2;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
robot_mode_ = ntohd(unpack_to);
if (version_ > 1.7) {
offset += sizeof(double);
joint_modes_ = unpackVector(buf, offset, 6);
}
}
if (version_ > 1.8) {
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
safety_mode_ = ntohd(unpack_to);
offset += sizeof(double);
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * 3;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
speed_scaling_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
linear_momentum_norm_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_main_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_robot_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
i_robot_ = ntohd(unpack_to);
offset += sizeof(double);
v_actual_ = unpackVector(buf, offset, 6);
}
val_lock_.unlock();
controller_updated_ = true;
data_published_ = true;
pMsg_cond_->notify_all();
}

32
src/ros/rt_publisher.cpp
View File

@@ -1,17 +1,18 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
sensor_msgs::JointState joint_msg;
bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = _joint_names;
for(auto const& q : packet.q_actual) {
for (auto const& q : packet.q_actual) {
joint_msg.position.push_back(q);
}
for(auto const& qd : packet.qd_actual) {
for (auto const& qd : packet.qd_actual) {
joint_msg.velocity.push_back(qd);
}
for(auto const& i : packet.i_actual) {
for (auto const& i : packet.i_actual) {
joint_msg.effort.push_back(i);
}
@@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
return true;
}
bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) {
bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
@@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) {
return true;
}
bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) {
bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
@@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) {
return true;
}
bool RTPublisher::publish(RTShared &packet) {
bool RTPublisher::publish(RTShared& packet)
{
ros::Time time = ros::Time::now();
return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7 &state) {
bool RTPublisher::consume(RTState_V1_6__7& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V1_8 &state) {
bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_0__1 &state) {
bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_2__3 &state) {
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
}

93
src/ros_main.cpp
View File

@@ -1,18 +1,18 @@
#include <cstdlib>
#include <string>
#include <chrono>
#include <thread>
#include <cstdlib>
#include <ros/ros.h>
#include <string>
#include <thread>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/producer.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/state.h"
static const std::string IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port");
@@ -21,48 +21,48 @@ static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
struct ProgArgs {
public:
std::string host;
std::string prefix;
std::string base_frame;
std::string tool_frame;
int32_t reverse_port;
bool use_sim_time;
std::string host;
std::string prefix;
std::string base_frame;
std::string tool_frame;
int32_t reverse_port;
bool use_sim_time;
};
bool parse_args(ProgArgs &args) {
if(!ros::param::get(IP_ADDR_ARG, args.host)) {
LOG_ERROR("robot_ip_address parameter must be set!");
return false;
}
ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
return true;
bool parse_args(ProgArgs& args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host)) {
LOG_ERROR("robot_ip_address parameter must be set!");
return false;
}
ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
return true;
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
ros::init(argc, argv, "ur_driver");
ProgArgs args;
if(!parse_args(args)) {
return EXIT_FAILURE;
}
ProgArgs args;
if (!parse_args(args)) {
return EXIT_FAILURE;
}
URFactory factory(args.host);
URFactory factory(args.host);
auto parser = factory.get_rt_parser();
auto parser = factory.get_rt_parser();
URStream s2(args.host, 30003);
URProducer<RTPacket> p2(s2, *parser);
URStream s2(args.host, 30003);
URProducer<RTPacket> p2(s2, *parser);
/*
/*
p2.setup_producer();
while(true) {
@@ -77,18 +77,17 @@ int main(int argc, char **argv) {
p2.teardown_producer();
*/
RTPublisher pub(args.prefix, args.base_frame);
RTPublisher pub(args.prefix, args.base_frame);
Pipeline<RTPacket> pl(p2, pub);
Pipeline<RTPacket> pl(p2, pub);
pl.run();
pl.run();
while(ros::ok()) {
std::this_thread::sleep_for(std::chrono::milliseconds(33));
}
while (ros::ok()) {
std::this_thread::sleep_for(std::chrono::milliseconds(33));
}
pl.stop();
pl.stop();
return EXIT_SUCCESS;
return EXIT_SUCCESS;
}

41
src/ur/master_board.cpp
View File

@@ -1,7 +1,8 @@
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedMasterBoardData::parse_with(BinParser &bp) {
bool SharedMasterBoardData::parse_with(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
@@ -17,8 +18,9 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
if(!bp.check_size<MasterBoardData_V1_X>())
bool MasterBoardData_V1_X::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V1_X>())
return false;
bp.parse(digital_input_bits);
@@ -30,8 +32,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if(euromap67_interface_installed) {
if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
@@ -41,8 +43,9 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
if(!bp.check_size<MasterBoardData_V3_0__1>())
bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_0__1>())
return false;
bp.parse(digital_input_bits);
@@ -54,8 +57,8 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
if(euromap67_interface_installed) {
if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
@@ -67,10 +70,9 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
if(!bp.check_size<MasterBoardData_V3_2>())
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parse_with(bp);
@@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
return true;
}
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) {
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) {
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}

8
src/ur/messages.cpp
View File

@@ -1,8 +1,8 @@
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.h"
bool VersionMessage::parse_with(BinParser &bp) {
bool VersionMessage::parse_with(BinParser& bp)
{
bp.parse(project_name);
bp.parse(major_version);
@@ -14,7 +14,7 @@ bool VersionMessage::parse_with(BinParser &bp) {
return true; //not really possible to check dynamic size packets
}
bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) {
bool VersionMessage::consume_with(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
}

33
src/ur/robot_mode.cpp
View File

@@ -1,8 +1,8 @@
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedRobotModeData::parse_with(BinParser &bp) {
bool SharedRobotModeData::parse_with(BinParser& bp)
{
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
@@ -11,9 +11,9 @@ bool SharedRobotModeData::parse_with(BinParser &bp) {
return true;
}
bool RobotModeData_V1_X::parse_with(BinParser &bp) {
if(!bp.check_size<RobotModeData_V1_X>())
bool RobotModeData_V1_X::parse_with(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V1_X>())
return false;
SharedRobotModeData::parse_with(bp);
@@ -27,9 +27,9 @@ bool RobotModeData_V1_X::parse_with(BinParser &bp) {
return true;
}
bool RobotModeData_V3_0__1::parse_with(BinParser &bp) {
if(!bp.check_size<RobotModeData_V3_0__1>())
bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_0__1>())
return false;
SharedRobotModeData::parse_with(bp);
@@ -45,8 +45,9 @@ bool RobotModeData_V3_0__1::parse_with(BinParser &bp) {
return true;
}
bool RobotModeData_V3_2::parse_with(BinParser &bp) {
if(!bp.check_size<RobotModeData_V3_2>())
bool RobotModeData_V3_2::parse_with(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_2>())
return false;
RobotModeData_V3_0__1::parse_with(bp);
@@ -56,15 +57,15 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) {
return true;
}
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) {
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) {
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}

40
src/ur/rt_state.cpp
View File

@@ -1,8 +1,8 @@
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ur/consumer.h"
bool RTShared::parse_shared1(BinParser &bp) {
bool RTShared::parse_shared1(BinParser& bp)
{
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
@@ -15,7 +15,8 @@ bool RTShared::parse_shared1(BinParser &bp) {
return true;
}
bool RTShared::parse_shared2(BinParser &bp) {
bool RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_input);
bp.parse(motor_temperatures);
bp.parse(controller_time);
@@ -23,9 +24,9 @@ bool RTShared::parse_shared2(BinParser &bp) {
return true;
}
bool RTState_V1_6__7::parse_with(BinParser &bp) {
if(!bp.check_size<RTState_V1_6__7>())
bool RTState_V1_6__7::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V1_6__7>())
return false;
parse_shared1(bp);
@@ -40,8 +41,9 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) {
return true;
}
bool RTState_V1_8::parse_with(BinParser &bp) {
if(!bp.check_size<RTState_V1_8>())
bool RTState_V1_8::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V1_8>())
return false;
RTState_V1_6__7::parse_with(bp);
@@ -51,8 +53,9 @@ bool RTState_V1_8::parse_with(BinParser &bp) {
return true;
}
bool RTState_V3_0__1::parse_with(BinParser &bp) {
if(!bp.check_size<RTState_V3_0__1>())
bool RTState_V3_0__1::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V3_0__1>())
return false;
parse_shared1(bp);
@@ -83,8 +86,9 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) {
return true;
}
bool RTState_V3_2__3::parse_with(BinParser &bp) {
if(!bp.check_size<RTState_V3_2__3>())
bool RTState_V3_2__3::parse_with(BinParser& bp)
{
if (!bp.check_size<RTState_V3_2__3>())
return false;
RTState_V3_0__1::parse_with(bp);
@@ -95,15 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser &bp) {
return true;
}
bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) {
bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) {
bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) {
bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) {
bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}

4
src/ur/state.cpp
View File

@@ -1,7 +1,5 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/log.h"
//StatePacket::~StatePacket() { }

58
src/ur/stream.cpp
View File

@@ -1,13 +1,14 @@
#include <cstring>
#include <unistd.h>
#include <netinet/tcp.h>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect() {
if(_initialized)
bool URStream::connect()
{
if (_initialized)
return false;
LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port);
@@ -23,20 +24,20 @@ bool URStream::connect() {
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
if (getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) {
LOG_ERROR("Failed to get host name");
return false;
}
//loop through the list of addresses untill we find one that's connectable
for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) {
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next) {
_socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if(_socket_fd == -1) //socket error?
if (_socket_fd == -1) //socket error?
continue;
if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if(_stopping)
if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if (_stopping)
break;
else
continue; //try next addrinfo if connect fails
@@ -51,14 +52,15 @@ bool URStream::connect() {
}
freeaddrinfo(result);
if(!_initialized)
if (!_initialized)
LOG_ERROR("Connection failed");
return _initialized;
}
void URStream::disconnect() {
if(!_initialized || _stopping)
void URStream::disconnect()
{
if (!_initialized || _stopping)
return;
_stopping = true;
@@ -66,10 +68,11 @@ void URStream::disconnect() {
_initialized = false;
}
ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
if(!_initialized)
ssize_t URStream::send(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
return -1;
if(_stopping)
if (_stopping)
return 0;
size_t total = 0;
@@ -77,9 +80,9 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
//TODO: handle reconnect?
//handle partial sends
while(total < buf_len) {
ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0);
if(sent <= 0)
while (total < buf_len) {
ssize_t sent = ::send(_socket_fd, buf + total, remaining, 0);
if (sent <= 0)
return _stopping ? 0 : sent;
total += sent;
remaining -= sent;
@@ -88,24 +91,25 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
return total;
}
ssize_t URStream::receive(uint8_t *buf, size_t buf_len) {
if(!_initialized)
ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
return -1;
if(_stopping)
if (_stopping)
return 0;
size_t remainder = sizeof(int32_t);
uint8_t *buf_pos = buf;
uint8_t* buf_pos = buf;
bool initial = true;
do {
ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
if(read <= 0) //failed reading from socket
if (read <= 0) //failed reading from socket
return _stopping ? 0 : read;
if(initial) {
if (initial) {
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if(remainder >= (buf_len - sizeof(int32_t))) {
if (remainder >= (buf_len - sizeof(int32_t))) {
LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len);
return -1;
}
@@ -114,7 +118,7 @@ ssize_t URStream::receive(uint8_t *buf, size_t buf_len) {
buf_pos += read;
remainder -= read;
} while(remainder > 0);
} while (remainder > 0);
return buf_pos - buf;
}

300
src/ur_communication.cpp
View File

@@ -19,163 +19,167 @@
#include "ur_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond,
std::string host) {
robot_state_ = new RobotState(msg_cond);
bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0) {
print_fatal("ERROR opening socket pri_sockfd");
}
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
print_fatal("ERROR opening socket sec_sockfd");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
print_fatal("ERROR, unknown host");
}
pri_serv_addr_.sin_family = AF_INET;
sec_serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char *) server_->h_addr, (char *)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
std::string host)
{
robot_state_ = new RobotState(msg_cond);
bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0) {
print_fatal("ERROR opening socket pri_sockfd");
}
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
print_fatal("ERROR opening socket sec_sockfd");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
print_fatal("ERROR, unknown host");
}
pri_serv_addr_.sin_family = AF_INET;
sec_serv_addr_.sin_family = AF_INET;
bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
}
bool UrCommunication::start() {
keepalive_ = true;
uint8_t buf[512];
unsigned int bytes_read;
std::string cmd;
bzero(buf, 512);
print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
sizeof(pri_serv_addr_)) < 0) {
print_fatal("Error connecting to get firmware version");
return false;
}
print_debug("Acquire firmware version: Got connection");
bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf, bytes_read);
//wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
char tmp[64];
sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
print_debug(tmp);
close(pri_sockfd_);
bool UrCommunication::start()
{
keepalive_ = true;
uint8_t buf[512];
unsigned int bytes_read;
std::string cmd;
bzero(buf, 512);
print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_,
sizeof(pri_serv_addr_))
< 0) {
print_fatal("Error connecting to get firmware version");
return false;
}
print_debug("Acquire firmware version: Got connection");
bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
robot_state_->unpack(buf, bytes_read);
//wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
char tmp[64];
sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
print_debug(tmp);
close(pri_sockfd_);
print_debug(
"Switching to secondary interface for masterboard data: Connecting...");
print_debug(
"Switching to secondary interface for masterboard data: Connecting...");
fd_set writefds;
struct timeval timeout;
fd_set writefds;
struct timeval timeout;
connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_fatal("Error connecting to secondary interface");
return false;
}
print_debug("Secondary interface: Got connection");
comThread_ = std::thread(&UrCommunication::run, this);
return true;
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_fatal("Error connecting to secondary interface");
return false;
}
print_debug("Secondary interface: Got connection");
comThread_ = std::thread(&UrCommunication::run, this);
return true;
}
void UrCommunication::halt() {
keepalive_ = false;
comThread_.join();
void UrCommunication::halt()
{
keepalive_ = false;
comThread_.join();
}
void UrCommunication::run() {
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sec_sockfd_, &readfds);
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
if (bytes_read > 0) {
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
(char *) &flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
} else {
connected_ = false;
robot_state_->setDisconnected();
close(sec_sockfd_);
}
}
if (keepalive_) {
//reconnect
print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
print_fatal("ERROR opening secondary socket");
}
flag_ = 1;
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
void UrCommunication::run()
{
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sec_sockfd_, &readfds);
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
if (bytes_read > 0) {
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK,
(char*)&flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
} else {
connected_ = false;
robot_state_->setDisconnected();
close(sec_sockfd_);
}
}
if (keepalive_) {
//reconnect
print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
print_fatal("ERROR opening secondary socket");
}
flag_ = 1;
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
&flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Secondary port: Reconnected");
}
}
}
}
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_,
sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
&flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Secondary port: Reconnected");
}
}
}
}
//wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sec_sockfd_);
//wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sec_sockfd_);
}

638
src/ur_driver.cpp
View File

@@ -19,364 +19,396 @@
#include "ur_modern_driver/ur_driver.h"
UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port, double servoj_time,
unsigned int safety_count_max, double max_time_step, double min_payload,
double max_payload, double servoj_lookahead_time, double servoj_gain) :
REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
min_payload), maximum_payload_(max_payload), servoj_time_(
servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) {
char buffer[256];
struct sockaddr_in serv_addr;
int n, flag;
std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port, double servoj_time,
unsigned int safety_count_max, double max_time_step, double min_payload,
double max_payload, double servoj_lookahead_time, double servoj_gain)
: REVERSE_PORT_(reverse_port)
, maximum_time_step_(max_time_step)
, minimum_payload_(
min_payload)
, maximum_payload_(max_payload)
, servoj_time_(
servoj_time)
, servoj_lookahead_time_(servoj_lookahead_time)
, servoj_gain_(servoj_gain)
{
char buffer[256];
struct sockaddr_in serv_addr;
int n, flag;
firmware_version_ = 0;
reverse_connected_ = false;
executing_traj_ = false;
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max);
new_sockfd_ = -1;
sec_interface_ = new UrCommunication(msg_cond, host);
firmware_version_ = 0;
reverse_connected_ = false;
executing_traj_ = false;
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max);
new_sockfd_ = -1;
sec_interface_ = new UrCommunication(msg_cond, host);
incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (incoming_sockfd_ < 0) {
print_fatal("ERROR opening socket for reverse communication");
}
bzero((char *) &serv_addr, sizeof(serv_addr));
incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (incoming_sockfd_ < 0) {
print_fatal("ERROR opening socket for reverse communication");
}
bzero((char*)&serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(REVERSE_PORT_);
flag = 1;
setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag,
sizeof(int));
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr,
sizeof(serv_addr)) < 0) {
print_fatal("ERROR on binding socket for reverse communication");
}
listen(incoming_sockfd_, 5);
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(REVERSE_PORT_);
flag = 1;
setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag,
sizeof(int));
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr,
sizeof(serv_addr))
< 0) {
print_fatal("ERROR on binding socket for reverse communication");
}
listen(incoming_sockfd_, 5);
}
std::vector<double> UrDriver::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) {
/*Returns positions of the joints at time 't' */
std::vector<double> positions;
for (unsigned int i = 0; i < p0_pos.size(); i++) {
double a = p0_pos[i];
double b = p0_vel[i];
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
- T * p1_vel[i]) / pow(T, 2);
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
+ T * p1_vel[i]) / pow(T, 3);
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
}
return positions;
std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel)
{
/*Returns positions of the joints at time 't' */
std::vector<double> positions;
for (unsigned int i = 0; i < p0_pos.size(); i++) {
double a = p0_pos[i];
double b = p0_vel[i];
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
- T * p1_vel[i])
/ pow(T, 2);
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
+ T * p1_vel[i])
/ pow(T, 3);
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
}
return positions;
}
bool UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j;
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities)
{
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j;
if (!UrDriver::uploadProg()) {
return false;
}
executing_traj_ = true;
t0 = std::chrono::high_resolution_clock::now();
t = t0;
j = 0;
while ((inp_timestamps[inp_timestamps.size() - 1]
>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
and executing_traj_) {
while (inp_timestamps[j]
<= std::chrono::duration_cast<std::chrono::duration<double>>(
t - t0).count() && j < inp_timestamps.size() - 1) {
j += 1;
}
positions = UrDriver::interp_cubic(
std::chrono::duration_cast<std::chrono::duration<double>>(
t - t0).count() - inp_timestamps[j - 1],
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
UrDriver::servoj(positions);
if (!UrDriver::uploadProg()) {
return false;
}
executing_traj_ = true;
t0 = std::chrono::high_resolution_clock::now();
t = t0;
j = 0;
while ((inp_timestamps[inp_timestamps.size() - 1]
>= std::chrono::duration_cast<std::chrono::duration<double> >(t - t0).count())
and executing_traj_) {
while (inp_timestamps[j]
<= std::chrono::duration_cast<std::chrono::duration<double> >(
t - t0)
.count()
&& j < inp_timestamps.size() - 1) {
j += 1;
}
positions = UrDriver::interp_cubic(
std::chrono::duration_cast<std::chrono::duration<double> >(
t - t0)
.count()
- inp_timestamps[j - 1],
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
UrDriver::servoj(positions);
// oversample with 4 * sample_time
std::this_thread::sleep_for(
std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now();
}
executing_traj_ = false;
//Signal robot to stop driverProg()
UrDriver::closeServo(positions);
return true;
// oversample with 4 * sample_time
std::this_thread::sleep_for(
std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now();
}
executing_traj_ = false;
//Signal robot to stop driverProg()
UrDriver::closeServo(positions);
return true;
}
void UrDriver::servoj(std::vector<double> positions, int keepalive) {
if (!reverse_connected_) {
print_error(
"UrDriver::servoj called without a reverse connection present. Keepalive: "
+ std::to_string(keepalive));
return;
}
unsigned int bytes_written;
int tmp;
unsigned char buf[28];
for (int i = 0; i < 6; i++) {
tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_));
buf[i * 4] = tmp & 0xff;
buf[i * 4 + 1] = (tmp >> 8) & 0xff;
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
}
tmp = htonl((int) keepalive);
buf[6 * 4] = tmp & 0xff;
buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
bytes_written = write(new_sockfd_, buf, 28);
void UrDriver::servoj(std::vector<double> positions, int keepalive)
{
if (!reverse_connected_) {
print_error(
"UrDriver::servoj called without a reverse connection present. Keepalive: "
+ std::to_string(keepalive));
return;
}
unsigned int bytes_written;
int tmp;
unsigned char buf[28];
for (int i = 0; i < 6; i++) {
tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
buf[i * 4] = tmp & 0xff;
buf[i * 4 + 1] = (tmp >> 8) & 0xff;
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
}
tmp = htonl((int)keepalive);
buf[6 * 4] = tmp & 0xff;
buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
bytes_written = write(new_sockfd_, buf, 28);
}
void UrDriver::stopTraj() {
executing_traj_ = false;
rt_interface_->addCommandToQueue("stopj(10)\n");
void UrDriver::stopTraj()
{
executing_traj_ = false;
rt_interface_->addCommandToQueue("stopj(10)\n");
}
bool UrDriver::uploadProg() {
std::string cmd_str;
char buf[128];
cmd_str = "def driverProg():\n";
bool UrDriver::uploadProg()
{
std::string cmd_str;
char buf[128];
cmd_str = "def driverProg():\n";
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
cmd_str += buf;
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
cmd_str += buf;
cmd_str += "\tSERVO_IDLE = 0\n";
cmd_str += "\tSERVO_RUNNING = 1\n";
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
cmd_str += "\tdef set_servo_setpoint(q):\n";
cmd_str += "\t\tenter_critical\n";
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
cmd_str += "\t\tcmd_servo_q = q\n";
cmd_str += "\t\texit_critical\n";
cmd_str += "\tend\n";
cmd_str += "\tthread servoThread():\n";
cmd_str += "\t\tstate = SERVO_IDLE\n";
cmd_str += "\t\twhile True:\n";
cmd_str += "\t\t\tenter_critical\n";
cmd_str += "\t\t\tq = cmd_servo_q\n";
cmd_str += "\t\t\tdo_brake = False\n";
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
cmd_str += "\t\t\t\tdo_brake = True\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\tstate = cmd_servo_state\n";
cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\t\t\texit_critical\n";
cmd_str += "\t\t\tif do_brake:\n";
cmd_str += "\t\t\t\tstopj(1.0)\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
cmd_str += "\tSERVO_IDLE = 0\n";
cmd_str += "\tSERVO_RUNNING = 1\n";
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
cmd_str += "\tdef set_servo_setpoint(q):\n";
cmd_str += "\t\tenter_critical\n";
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
cmd_str += "\t\tcmd_servo_q = q\n";
cmd_str += "\t\texit_critical\n";
cmd_str += "\tend\n";
cmd_str += "\tthread servoThread():\n";
cmd_str += "\t\tstate = SERVO_IDLE\n";
cmd_str += "\t\twhile True:\n";
cmd_str += "\t\t\tenter_critical\n";
cmd_str += "\t\t\tq = cmd_servo_q\n";
cmd_str += "\t\t\tdo_brake = False\n";
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
cmd_str += "\t\t\t\tdo_brake = True\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\tstate = cmd_servo_state\n";
cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\t\t\texit_critical\n";
cmd_str += "\t\t\tif do_brake:\n";
cmd_str += "\t\t\t\tstopj(1.0)\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
if (sec_interface_->robot_state_->getVersion() >= 3.1)
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
servoj_time_, servoj_lookahead_time_, servoj_gain_);
else
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
cmd_str += buf;
if (sec_interface_->robot_state_->getVersion() >= 3.1)
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
servoj_time_, servoj_lookahead_time_, servoj_gain_);
else
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
cmd_str += buf;
cmd_str += "\t\t\telse:\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\t\t\telse:\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
REVERSE_PORT_);
cmd_str += buf;
sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
REVERSE_PORT_);
cmd_str += buf;
cmd_str += "\tthread_servo = run servoThread()\n";
cmd_str += "\tkeepalive = 1\n";
cmd_str += "\twhile keepalive > 0:\n";
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
cmd_str += "\t\tif params_mult[0] > 0:\n";
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
cmd_str += "params_mult[2] / MULT_jointstate, ";
cmd_str += "params_mult[3] / MULT_jointstate, ";
cmd_str += "params_mult[4] / MULT_jointstate, ";
cmd_str += "params_mult[5] / MULT_jointstate, ";
cmd_str += "params_mult[6] / MULT_jointstate]\n";
cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
cmd_str += "\t\t\tset_servo_setpoint(q)\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\tsleep(.1)\n";
cmd_str += "\tsocket_close()\n";
cmd_str += "\tkill thread_servo\n";
cmd_str += "end\n";
cmd_str += "\tthread_servo = run servoThread()\n";
cmd_str += "\tkeepalive = 1\n";
cmd_str += "\twhile keepalive > 0:\n";
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
cmd_str += "\t\tif params_mult[0] > 0:\n";
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
cmd_str += "params_mult[2] / MULT_jointstate, ";
cmd_str += "params_mult[3] / MULT_jointstate, ";
cmd_str += "params_mult[4] / MULT_jointstate, ";
cmd_str += "params_mult[5] / MULT_jointstate, ";
cmd_str += "params_mult[6] / MULT_jointstate]\n";
cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
cmd_str += "\t\t\tset_servo_setpoint(q)\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\tsleep(.1)\n";
cmd_str += "\tsocket_close()\n";
cmd_str += "\tkill thread_servo\n";
cmd_str += "end\n";
rt_interface_->addCommandToQueue(cmd_str);
return UrDriver::openServo();
rt_interface_->addCommandToQueue(cmd_str);
return UrDriver::openServo();
}
bool UrDriver::openServo() {
struct sockaddr_in cli_addr;
socklen_t clilen;
clilen = sizeof(cli_addr);
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr,
&clilen);
if (new_sockfd_ < 0) {
print_fatal("ERROR on accepting reverse communication");
return false;
}
reverse_connected_ = true;
return true;
bool UrDriver::openServo()
{
struct sockaddr_in cli_addr;
socklen_t clilen;
clilen = sizeof(cli_addr);
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr,
&clilen);
if (new_sockfd_ < 0) {
print_fatal("ERROR on accepting reverse communication");
return false;
}
reverse_connected_ = true;
return true;
}
void UrDriver::closeServo(std::vector<double> positions) {
if (positions.size() != 6)
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
else
UrDriver::servoj(positions, 0);
void UrDriver::closeServo(std::vector<double> positions)
{
if (positions.size() != 6)
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
else
UrDriver::servoj(positions, 0);
reverse_connected_ = false;
close(new_sockfd_);
reverse_connected_ = false;
close(new_sockfd_);
}
bool UrDriver::start() {
if (!sec_interface_->start())
return false;
firmware_version_ = sec_interface_->robot_state_->getVersion();
rt_interface_->robot_state_->setVersion(firmware_version_);
if (!rt_interface_->start())
return false;
ip_addr_ = rt_interface_->getLocalIp();
print_debug(
"Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
+ "\n");
return true;
bool UrDriver::start()
{
if (!sec_interface_->start())
return false;
firmware_version_ = sec_interface_->robot_state_->getVersion();
rt_interface_->robot_state_->setVersion(firmware_version_);
if (!rt_interface_->start())
return false;
ip_addr_ = rt_interface_->getLocalIp();
print_debug(
"Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
+ "\n");
return true;
}
void UrDriver::halt() {
if (executing_traj_) {
UrDriver::stopTraj();
}
sec_interface_->halt();
rt_interface_->halt();
close(incoming_sockfd_);
void UrDriver::halt()
{
if (executing_traj_) {
UrDriver::stopTraj();
}
sec_interface_->halt();
rt_interface_->halt();
close(incoming_sockfd_);
}
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc) {
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
double q5, double acc)
{
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
}
std::vector<std::string> UrDriver::getJointNames() {
return joint_names_;
std::vector<std::string> UrDriver::getJointNames()
{
return joint_names_;
}
void UrDriver::setJointNames(std::vector<std::string> jn) {
joint_names_ = jn;
void UrDriver::setJointNames(std::vector<std::string> jn)
{
joint_names_ = jn;
}
void UrDriver::setToolVoltage(unsigned int v) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
void UrDriver::setToolVoltage(unsigned int v)
{
char buf[256];
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setFlag(unsigned int n, bool b) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
b ? "True" : "False");
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
void UrDriver::setFlag(unsigned int n, bool b)
{
char buf[256];
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
b ? "True" : "False");
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setDigitalOut(unsigned int n, bool b) {
char buf[256];
if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
b ? "True" : "False");
void UrDriver::setDigitalOut(unsigned int n, bool b)
{
char buf[256];
if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
b ? "True" : "False");
} else if (n > 15) {
sprintf(buf,
"sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
n - 16, b ? "True" : "False");
} else if (n > 7) {
"sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
n - 16, b ? "True" : "False");
} else if (n > 7) {
sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
n - 8, b ? "True" : "False");
} else {
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
n, b ? "True" : "False");
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
n - 8, b ? "True" : "False");
} else {
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
n, b ? "True" : "False");
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setAnalogOut(unsigned int n, double f) {
char buf[256];
if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
} else {
sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
}
void UrDriver::setAnalogOut(unsigned int n, double f)
{
char buf[256];
if (firmware_version_ < 2) {
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
} else {
sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
bool UrDriver::setPayload(double m) {
if ((m < maximum_payload_) && (m > minimum_payload_)) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
return true;
} else
return false;
bool UrDriver::setPayload(double m)
{
if ((m < maximum_payload_) && (m > minimum_payload_)) {
char buf[256];
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
return true;
} else
return false;
}
void UrDriver::setMinPayload(double m) {
if (m > 0) {
minimum_payload_ = m;
} else {
minimum_payload_ = 0;
}
void UrDriver::setMinPayload(double m)
{
if (m > 0) {
minimum_payload_ = m;
} else {
minimum_payload_ = 0;
}
}
void UrDriver::setMaxPayload(double m) {
maximum_payload_ = m;
void UrDriver::setMaxPayload(double m)
{
maximum_payload_ = m;
}
void UrDriver::setServojTime(double t) {
if (t > 0.008) {
servoj_time_ = t;
} else {
servoj_time_ = 0.008;
}
void UrDriver::setServojTime(double t)
{
if (t > 0.008) {
servoj_time_ = t;
} else {
servoj_time_ = 0.008;
}
}
void UrDriver::setServojLookahead(double t){
if (t > 0.03) {
if (t < 0.2) {
servoj_lookahead_time_ = t;
} else {
servoj_lookahead_time_ = 0.2;
}
} else {
servoj_lookahead_time_ = 0.03;
}
void UrDriver::setServojLookahead(double t)
{
if (t > 0.03) {
if (t < 0.2) {
servoj_lookahead_time_ = t;
} else {
servoj_lookahead_time_ = 0.2;
}
} else {
servoj_lookahead_time_ = 0.03;
}
}
void UrDriver::setServojGain(double g){
if (g > 100) {
if (g < 2000) {
servoj_gain_ = g;
} else {
servoj_gain_ = 2000;
}
} else {
servoj_gain_ = 100;
}
void UrDriver::setServojGain(double g)
{
if (g > 100) {
if (g < 2000) {
servoj_gain_ = g;
} else {
servoj_gain_ = 2000;
}
} else {
servoj_gain_ = 100;
}
}

389
src/ur_hardware_interface.cpp
View File

@@ -59,225 +59,226 @@
namespace ros_control_ur {
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) :
nh_(nh), robot_(robot) {
// Initialize shared memory and interfaces here
init(); // this implementation loads from rosparam
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
: nh_(nh)
, robot_(robot)
{
// Initialize shared memory and interfaces here
init(); // this implementation loads from rosparam
max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
}
void UrHardwareInterface::init() {
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
void UrHardwareInterface::init()
{
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?"
<< " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
prev_joint_velocity_command_.resize(num_joints_);
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
prev_joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
prev_joint_velocity_command_[i] = 0.;
}
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
prev_joint_velocity_command_[i] = 0.;
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
void UrHardwareInterface::read() {
std::vector<double> pos, vel, current, tcp;
pos = robot_->rt_interface_->robot_state_->getQActual();
vel = robot_->rt_interface_->robot_state_->getQdActual();
current = robot_->rt_interface_->robot_state_->getIActual();
tcp = robot_->rt_interface_->robot_state_->getTcpForce();
for (std::size_t i = 0; i < num_joints_; ++i) {
joint_position_[i] = pos[i];
joint_velocity_[i] = vel[i];
joint_effort_[i] = current[i];
}
for (std::size_t i = 0; i < 3; ++i) {
robot_force_[i] = tcp[i];
robot_torque_[i] = tcp[i + 3];
}
void UrHardwareInterface::read()
{
std::vector<double> pos, vel, current, tcp;
pos = robot_->rt_interface_->robot_state_->getQActual();
vel = robot_->rt_interface_->robot_state_->getQdActual();
current = robot_->rt_interface_->robot_state_->getIActual();
tcp = robot_->rt_interface_->robot_state_->getTcpForce();
for (std::size_t i = 0; i < num_joints_; ++i) {
joint_position_[i] = pos[i];
joint_velocity_[i] = vel[i];
joint_effort_[i] = current[i];
}
for (std::size_t i = 0; i < 3; ++i) {
robot_force_[i] = tcp[i];
robot_torque_[i] = tcp[i + 3];
}
}
void UrHardwareInterface::setMaxVelChange(double inp) {
max_vel_change_ = inp;
void UrHardwareInterface::setMaxVelChange(double inp)
{
max_vel_change_ = inp;
}
void UrHardwareInterface::write() {
if (velocity_interface_running_) {
std::vector<double> cmd;
//do some rate limiting
cmd.resize(joint_velocity_command_.size());
for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
cmd[i] = joint_velocity_command_[i];
if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
} else if (cmd[i]
< prev_joint_velocity_command_[i] - max_vel_change_) {
cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
}
prev_joint_velocity_command_[i] = cmd[i];
}
robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_*125);
} else if (position_interface_running_) {
robot_->servoj(joint_position_command_);
}
void UrHardwareInterface::write()
{
if (velocity_interface_running_) {
std::vector<double> cmd;
//do some rate limiting
cmd.resize(joint_velocity_command_.size());
for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
cmd[i] = joint_velocity_command_[i];
if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
} else if (cmd[i]
< prev_joint_velocity_command_[i] - max_vel_change_) {
cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
}
prev_joint_velocity_command_[i] = cmd[i];
}
robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
} else if (position_interface_running_) {
robot_->servoj(joint_position_command_);
}
}
bool UrHardwareInterface::canSwitch(
const std::list<hardware_interface::ControllerInfo> &start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
if (velocity_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (position_interface_running_) {
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it =
stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
}
} else if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
if (position_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (velocity_interface_running_) {
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it =
stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
}
}
}
const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) const
{
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
if (velocity_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (position_interface_running_) {
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
}
} else if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
if (position_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (velocity_interface_running_) {
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
error = false;
break;
}
}
if (error) {
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
}
}
}
// we can always stop a controller
return true;
// we can always stop a controller
return true;
}
void UrHardwareInterface::doSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
stop_list.begin(); controller_it != stop_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = false;
std::vector<double> tmp;
robot_->closeServo(tmp);
ROS_DEBUG("Stopping position interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = true;
robot_->uploadProg();
ROS_DEBUG("Starting position interface");
}
}
const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = false;
std::vector<double> tmp;
robot_->closeServo(tmp);
ROS_DEBUG("Stopping position interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
if (controller_it->hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = true;
robot_->uploadProg();
ROS_DEBUG("Starting position interface");
}
}
}
} // namespace

311
src/ur_realtime_communication.cpp
View File

@@ -19,175 +19,182 @@
#include "ur_modern_driver/ur_realtime_communication.h"
UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max) {
robot_state_ = new RobotStateRT(msg_cond);
bzero((char *) &serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
print_fatal("ERROR, no such host");
}
serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
serv_addr_.sin_port = htons(30003);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
safety_count_ = safety_count_max + 1;
safety_count_max_ = safety_count_max;
std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max)
{
robot_state_ = new RobotStateRT(msg_cond);
bzero((char*)&serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
print_fatal("ERROR, no such host");
}
serv_addr_.sin_family = AF_INET;
bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
serv_addr_.sin_port = htons(30003);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
safety_count_ = safety_count_max + 1;
safety_count_max_ = safety_count_max;
}
bool UrRealtimeCommunication::start() {
fd_set writefds;
struct timeval timeout;
bool UrRealtimeCommunication::start()
{
fd_set writefds;
struct timeval timeout;
keepalive_ = true;
print_debug("Realtime port: Connecting...");
keepalive_ = true;
print_debug("Realtime port: Connecting...");
connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_fatal("Error connecting to RT port 30003");
return false;
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
if (err < 0) {
print_fatal("Could not get local IP");
close(sockfd_);
return false;
}
char str[18];
inet_ntop(AF_INET, &name.sin_addr, str, 18);
local_ip_ = str;
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
return true;
connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_fatal("Error connecting to RT port 30003");
return false;
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
if (err < 0) {
print_fatal("Could not get local IP");
close(sockfd_);
return false;
}
char str[18];
inet_ntop(AF_INET, &name.sin_addr, str, 18);
local_ip_ = str;
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
return true;
}
void UrRealtimeCommunication::halt() {
keepalive_ = false;
comThread_.join();
void UrRealtimeCommunication::halt()
{
keepalive_ = false;
comThread_.join();
}
void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
int bytes_written;
if (inp.back() != '\n') {
inp.append("\n");
}
if (connected_)
bytes_written = write(sockfd_, inp.c_str(), inp.length());
else
print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" );
void UrRealtimeCommunication::addCommandToQueue(std::string inp)
{
int bytes_written;
if (inp.back() != '\n') {
inp.append("\n");
}
if (connected_)
bytes_written = write(sockfd_, inp.c_str(), inp.length());
else
print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
}
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
double q3, double q4, double q5, double acc) {
char cmd[1024];
if( robot_state_->getVersion() >= 3.1 ) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc);
}
else {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
q0, q1, q2, q3, q4, q5, acc);
}
addCommandToQueue((std::string) (cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
//If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
double q3, double q4, double q5, double acc)
{
char cmd[1024];
if (robot_state_->getVersion() >= 3.1) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc);
} else {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
q0, q1, q2, q3, q4, q5, acc);
}
addCommandToQueue((std::string)(cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
//If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
}
void UrRealtimeCommunication::run() {
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sockfd_, &readfds);
print_debug("Realtime port: Got connection");
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sockfd_, buf, 2048);
if (bytes_read > 0) {
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_) {
setSpeed(0., 0., 0., 0., 0., 0.);
}
safety_count_ += 1;
} else {
connected_ = false;
close(sockfd_);
}
}
if (keepalive_) {
//reconnect
print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_,
sizeof(int));
void UrRealtimeCommunication::run()
{
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sockfd_, &readfds);
print_debug("Realtime port: Got connection");
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sockfd_, buf, 2048);
if (bytes_read > 0) {
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_) {
setSpeed(0., 0., 0., 0., 0., 0.);
}
safety_count_ += 1;
} else {
connected_ = false;
close(sockfd_);
}
}
if (keepalive_) {
//reconnect
print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_,
sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_,
sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sockfd_, (struct sockaddr *) &serv_addr_,
sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Realtime port: Reconnected");
}
}
}
}
setSpeed(0., 0., 0., 0., 0., 0.);
close(sockfd_);
connect(sockfd_, (struct sockaddr*)&serv_addr_,
sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Realtime port: Reconnected");
}
}
}
}
setSpeed(0., 0., 0., 0., 0., 0.);
close(sockfd_);
}
void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
safety_count_max_ = inp;
void UrRealtimeCommunication::setSafetyCountMax(uint inp)
{
safety_count_max_ = inp;
}
std::string UrRealtimeCommunication::getLocalIp() {
return local_ip_;
std::string UrRealtimeCommunication::getLocalIp()
{
return local_ip_;
}

1356
src/ur_ros_wrapper.cpp
View File

File diff suppressed because it is too large Load Diff