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

Added clang formatting

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

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 #pragma once
#include <inttypes.h> #include "ur_modern_driver/log.h"
#include <endian.h> #include "ur_modern_driver/types.h"
#include <assert.h>
#include <cstddef> #include <cstddef>
#include <cstring> #include <cstring>
#include <endian.h>
#include <inttypes.h>
#include <string> #include <string>
#include <assert.h>
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/log.h"
class BinParser { class BinParser {
private: private:
uint8_t *_buf_pos, *_buf_end; uint8_t *_buf_pos, *_buf_end;
BinParser &_parent; BinParser& _parent;
public: public:
BinParser(uint8_t *buffer, size_t buf_len) : BinParser(uint8_t* buffer, size_t buf_len)
_buf_pos(buffer), : _buf_pos(buffer)
_buf_end(buffer+buf_len), , _buf_end(buffer + buf_len)
_parent(*this) { , _parent(*this)
assert(_buf_pos <= _buf_end); {
} assert(_buf_pos <= _buf_end);
}
BinParser(BinParser &parent, size_t sub_len) : BinParser(BinParser& parent, size_t sub_len)
_buf_pos(parent._buf_pos), : _buf_pos(parent._buf_pos)
_buf_end(parent._buf_pos+sub_len), , _buf_end(parent._buf_pos + sub_len)
_parent(parent) { , _parent(parent)
assert(_buf_pos <= _buf_end); {
} assert(_buf_pos <= _buf_end);
}
~BinParser() { ~BinParser()
{
_parent._buf_pos = _buf_pos; _parent._buf_pos = _buf_pos;
} }
//Decode from network encoding (big endian) to host encoding //Decode from network encoding (big endian) to host encoding
template<typename T> template <typename T>
T decode(T val) { T decode(T val)
{
return val; return val;
} }
uint16_t decode(uint16_t val) { uint16_t decode(uint16_t val)
{
return be16toh(val); return be16toh(val);
} }
uint32_t decode(uint32_t val) { uint32_t decode(uint32_t val)
{
return be32toh(val); return be32toh(val);
} }
uint64_t decode(uint64_t val) { uint64_t decode(uint64_t val)
{
return be64toh(val); return be64toh(val);
} }
int16_t decode(int16_t val) { int16_t decode(int16_t val)
{
return be16toh(val); return be16toh(val);
} }
int32_t decode(int32_t val) { int32_t decode(int32_t val)
{
return be32toh(val); return be32toh(val);
} }
int64_t decode(int64_t val) { int64_t decode(int64_t val)
{
return be64toh(val); return be64toh(val);
} }
float decode(float val) { float decode(float val)
{
return be32toh(val); return be32toh(val);
} }
double decode(double val) { double decode(double val)
{
return be64toh(val); return be64toh(val);
} }
template <typename T>
template<typename T> T peek()
T peek() { {
assert(_buf_pos <= _buf_end); assert(_buf_pos <= _buf_end);
return decode(*(reinterpret_cast<T*>(_buf_pos))); return decode(*(reinterpret_cast<T*>(_buf_pos)));
} }
template<typename T> template <typename T>
void parse(T &val) { void parse(T& val)
{
val = peek<T>(); val = peek<T>();
_buf_pos += sizeof(T); _buf_pos += sizeof(T);
} }
// UR uses 1 byte for boolean values but sizeof(bool) is implementation // UR uses 1 byte for boolean values but sizeof(bool) is implementation
// defined so we must ensure they're parsed as uint8_t on all compilers // 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; uint8_t inner;
parse<uint8_t>(inner); parse<uint8_t>(inner);
val = inner != 0; val = inner != 0;
} }
// Explicit parsing order of fields to avoid issues with struct layout // 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.x);
parse(val.y); parse(val.y);
parse(val.z); parse(val.z);
} }
// Explicit parsing order of fields to avoid issues with struct layout // 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.position);
parse(val.rotation); parse(val.rotation);
} }
void parse_remainder(std::string &val) { void parse_remainder(std::string& val)
{
parse(val, size_t(_buf_end - _buf_pos)); 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); val.assign(reinterpret_cast<char*>(_buf_pos), len);
_buf_pos += len; _buf_pos += len;
} }
// Special string parse function that assumes uint8_t len followed by chars // 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; uint8_t len;
parse(len); parse(len);
parse(val, size_t(len)); parse(val, size_t(len));
} }
template<typename T, size_t N> template <typename T, size_t N>
void parse(T (&array)[N]) { void parse(T (&array)[N])
for(size_t i = 0; i < N; i++) { {
for (size_t i = 0; i < N; i++) {
parse(array[i]); parse(array[i]);
} }
} }
void consume() { void consume()
{
_buf_pos = _buf_end; _buf_pos = _buf_end;
} }
void consume(size_t bytes) { void consume(size_t bytes)
{
_buf_pos += bytes; _buf_pos += bytes;
} }
bool check_size(size_t bytes) { bool check_size(size_t bytes)
{
return bytes <= size_t(_buf_end - _buf_pos); return bytes <= size_t(_buf_end - _buf_pos);
} }
template<typename T> template <typename T>
bool check_size(void) { bool check_size(void)
{
return check_size(T::SIZE); return check_size(T::SIZE);
} }
bool empty() { bool empty()
{
return _buf_pos == _buf_end; return _buf_pos == _buf_end;
} }
void debug() { void debug()
{
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos); 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_error(std::string inp);
void print_fatal(std::string inp); void print_fatal(std::string inp);
#endif /* UR_DO_OUTPUT_H_ */ #endif /* UR_DO_OUTPUT_H_ */

22
include/ur_modern_driver/log.h
View File

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

2
include/ur_modern_driver/packet.h
View File

@@ -3,5 +3,5 @@
class Packet { class Packet {
public: 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 { class Parser {
public: 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 #pragma once
#include <thread>
#include <atomic>
#include <vector>
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/queue/readerwriterqueue.h" #include "ur_modern_driver/queue/readerwriterqueue.h"
#include <atomic>
#include <thread>
#include <vector>
using namespace moodycamel; using namespace moodycamel;
using namespace std; using namespace std;
template <typename T> template <typename T>
class IConsumer { class IConsumer {
public: public:
virtual void setup_consumer() { } virtual void setup_consumer() {}
virtual void teardown_consumer() { } virtual void teardown_consumer() {}
virtual void stop_consumer() { } virtual void stop_consumer() {}
virtual bool consume(unique_ptr<T> product) = 0; virtual bool consume(unique_ptr<T> product) = 0;
}; };
@@ -23,33 +22,33 @@ public:
template <typename T> template <typename T>
class IProducer { class IProducer {
public: public:
virtual void setup_producer() { } virtual void setup_producer() {}
virtual void teardown_producer() { } virtual void teardown_producer() {}
virtual void stop_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> template <typename T>
class Pipeline { class Pipeline {
private: private:
IProducer<T> &_producer; IProducer<T>& _producer;
IConsumer<T> &_consumer; IConsumer<T>& _consumer;
BlockingReaderWriterQueue<unique_ptr<T>> _queue; BlockingReaderWriterQueue<unique_ptr<T> > _queue;
atomic<bool> _running; atomic<bool> _running;
thread _pThread, _cThread; thread _pThread, _cThread;
void run_producer() { void run_producer()
{
_producer.setup_producer(); _producer.setup_producer();
std::vector<unique_ptr<T>> products; std::vector<unique_ptr<T> > products;
while(_running) { while (_running) {
if(!_producer.try_get(products)) { if (!_producer.try_get(products)) {
break; break;
} }
for(auto &p : products) { for (auto& p : products) {
if(!_queue.try_enqueue(std::move(p))) { if (!_queue.try_enqueue(std::move(p))) {
LOG_WARN("Pipeline owerflowed!"); LOG_WARN("Pipeline owerflowed!");
} }
} }
@@ -60,27 +59,31 @@ private:
//todo cleanup //todo cleanup
} }
void run_consumer() { void run_consumer()
{
_consumer.setup_consumer(); _consumer.setup_consumer();
unique_ptr<T> product; unique_ptr<T> product;
while(_running) { while (_running) {
_queue.wait_dequeue(product); _queue.wait_dequeue(product);
if(!_consumer.consume(std::move(product))) if (!_consumer.consume(std::move(product)))
break; break;
} }
_consumer.teardown_consumer(); _consumer.teardown_consumer();
//todo cleanup //todo cleanup
} }
public:
Pipeline(IProducer<T> &producer, IConsumer<T> &consumer)
: _producer(producer),
_consumer(consumer),
_queue{32},
_running{false}
{ }
void run() { public:
if(_running) Pipeline(IProducer<T>& producer, IConsumer<T>& consumer)
: _producer(producer)
, _consumer(consumer)
, _queue{ 32 }
, _running{ false }
{
}
void run()
{
if (_running)
return; return;
_running = true; _running = true;
@@ -88,8 +91,9 @@ public:
_cThread = thread(&Pipeline::run_consumer, this); _cThread = thread(&Pipeline::run_consumer, this);
} }
void stop() { void stop()
if(!_running) {
if (!_running)
return; return;
_consumer.stop_consumer(); _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_ #ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_ #define ROBOT_STATE_H_
#include <condition_variable>
#include <inttypes.h> #include <inttypes.h>
#include <vector> #include <mutex>
#include <netinet/in.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <mutex> #include <vector>
#include <condition_variable>
#include <netinet/in.h>
namespace message_types { namespace message_types {
enum message_type { 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; typedef message_types::message_type messageType;
namespace package_types { namespace package_types {
enum package_type { enum package_type {
ROBOT_MODE_DATA = 0, ROBOT_MODE_DATA = 0,
JOINT_DATA = 1, JOINT_DATA = 1,
TOOL_DATA = 2, TOOL_DATA = 2,
MASTERBOARD_DATA = 3, MASTERBOARD_DATA = 3,
CARTESIAN_INFO = 4, CARTESIAN_INFO = 4,
KINEMATICS_INFO = 5, KINEMATICS_INFO = 5,
CONFIGURATION_DATA = 6, CONFIGURATION_DATA = 6,
FORCE_MODE_DATA = 7, FORCE_MODE_DATA = 7,
ADDITIONAL_INFO = 8, ADDITIONAL_INFO = 8,
CALIBRATION_DATA = 9 CALIBRATION_DATA = 9
}; };
} }
typedef package_types::package_type packageType; typedef package_types::package_type packageType;
namespace robot_message_types { namespace robot_message_types {
enum robot_message_type { enum robot_message_type {
ROBOT_MESSAGE_TEXT = 0, ROBOT_MESSAGE_TEXT = 0,
ROBOT_MESSAGE_PROGRAM_LABEL = 1, ROBOT_MESSAGE_PROGRAM_LABEL = 1,
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
ROBOT_MESSAGE_VERSION = 3, ROBOT_MESSAGE_VERSION = 3,
ROBOT_MESSAGE_SAFETY_MODE = 5, ROBOT_MESSAGE_SAFETY_MODE = 5,
ROBOT_MESSAGE_ERROR_CODE = 6, ROBOT_MESSAGE_ERROR_CODE = 6,
ROBOT_MESSAGE_KEY = 7, ROBOT_MESSAGE_KEY = 7,
ROBOT_MESSAGE_REQUEST_VALUE = 9, ROBOT_MESSAGE_REQUEST_VALUE = 9,
ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
}; };
} }
typedef robot_message_types::robot_message_type robotMessageType; typedef robot_message_types::robot_message_type robotMessageType;
namespace robot_state_type_v18 { namespace robot_state_type_v18 {
enum robot_state_type { enum robot_state_type {
ROBOT_RUNNING_MODE = 0, ROBOT_RUNNING_MODE = 0,
ROBOT_FREEDRIVE_MODE = 1, ROBOT_FREEDRIVE_MODE = 1,
ROBOT_READY_MODE = 2, ROBOT_READY_MODE = 2,
ROBOT_INITIALIZING_MODE = 3, ROBOT_INITIALIZING_MODE = 3,
ROBOT_SECURITY_STOPPED_MODE = 4, ROBOT_SECURITY_STOPPED_MODE = 4,
ROBOT_EMERGENCY_STOPPED_MODE = 5, ROBOT_EMERGENCY_STOPPED_MODE = 5,
ROBOT_FATAL_ERROR_MODE = 6, ROBOT_FATAL_ERROR_MODE = 6,
ROBOT_NO_POWER_MODE = 7, ROBOT_NO_POWER_MODE = 7,
ROBOT_NOT_CONNECTED_MODE = 8, ROBOT_NOT_CONNECTED_MODE = 8,
ROBOT_SHUTDOWN_MODE = 9, ROBOT_SHUTDOWN_MODE = 9,
ROBOT_SAFEGUARD_STOP_MODE = 10 ROBOT_SAFEGUARD_STOP_MODE = 10
}; };
} }
typedef robot_state_type_v18::robot_state_type robotStateTypeV18; typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
namespace robot_state_type_v30 { namespace robot_state_type_v30 {
enum robot_state_type { enum robot_state_type {
ROBOT_MODE_DISCONNECTED = 0, ROBOT_MODE_DISCONNECTED = 0,
ROBOT_MODE_CONFIRM_SAFETY = 1, ROBOT_MODE_CONFIRM_SAFETY = 1,
ROBOT_MODE_BOOTING = 2, ROBOT_MODE_BOOTING = 2,
ROBOT_MODE_POWER_OFF = 3, ROBOT_MODE_POWER_OFF = 3,
ROBOT_MODE_POWER_ON = 4, ROBOT_MODE_POWER_ON = 4,
ROBOT_MODE_IDLE = 5, ROBOT_MODE_IDLE = 5,
ROBOT_MODE_BACKDRIVE = 6, ROBOT_MODE_BACKDRIVE = 6,
ROBOT_MODE_RUNNING = 7, ROBOT_MODE_RUNNING = 7,
ROBOT_MODE_UPDATING_FIRMWARE = 8 ROBOT_MODE_UPDATING_FIRMWARE = 8
}; };
} }
typedef robot_state_type_v30::robot_state_type robotStateTypeV30; typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
struct version_message { struct version_message {
uint64_t timestamp; uint64_t timestamp;
int8_t source; int8_t source;
int8_t robot_message_type; int8_t robot_message_type;
int8_t project_name_size; int8_t project_name_size;
char project_name[15]; char project_name[15];
uint8_t major_version; uint8_t major_version;
uint8_t minor_version; uint8_t minor_version;
int svn_revision; int svn_revision;
char build_date[25]; char build_date[25];
}; };
struct masterboard_data { struct masterboard_data {
int digitalInputBits; int digitalInputBits;
int digitalOutputBits; int digitalOutputBits;
char analogInputRange0; char analogInputRange0;
char analogInputRange1; char analogInputRange1;
double analogInput0; double analogInput0;
double analogInput1; double analogInput1;
char analogOutputDomain0; char analogOutputDomain0;
char analogOutputDomain1; char analogOutputDomain1;
double analogOutput0; double analogOutput0;
double analogOutput1; double analogOutput1;
float masterBoardTemperature; float masterBoardTemperature;
float robotVoltage48V; float robotVoltage48V;
float robotCurrent; float robotCurrent;
float masterIOCurrent; float masterIOCurrent;
unsigned char safetyMode; unsigned char safetyMode;
unsigned char masterOnOffState; unsigned char masterOnOffState;
char euromap67InterfaceInstalled; char euromap67InterfaceInstalled;
int euromapInputBits; int euromapInputBits;
int euromapOutputBits; int euromapOutputBits;
float euromapVoltage; float euromapVoltage;
float euromapCurrent; float euromapCurrent;
}; };
struct robot_mode_data { struct robot_mode_data {
uint64_t timestamp; uint64_t timestamp;
bool isRobotConnected; bool isRobotConnected;
bool isRealRobotEnabled; bool isRealRobotEnabled;
bool isPowerOnRobot; bool isPowerOnRobot;
bool isEmergencyStopped; bool isEmergencyStopped;
bool isProtectiveStopped; bool isProtectiveStopped;
bool isProgramRunning; bool isProgramRunning;
bool isProgramPaused; bool isProgramPaused;
unsigned char robotMode; unsigned char robotMode;
unsigned char controlMode; unsigned char controlMode;
double targetSpeedFraction; double targetSpeedFraction;
double speedScaling; double speedScaling;
}; };
class RobotState { class RobotState {
private: private:
version_message version_msg_; version_message version_msg_;
masterboard_data mb_data_; masterboard_data mb_data_;
robot_mode_data robot_mode_; 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 std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool new_data_available_; //to avoid spurious wakes bool new_data_available_; //to avoid spurious wakes
unsigned char robot_mode_running_; unsigned char robot_mode_running_;
double ntohd(uint64_t nf); double ntohd(uint64_t nf);
public: public:
RobotState(std::condition_variable& msg_cond); RobotState(std::condition_variable& msg_cond);
~RobotState(); ~RobotState();
double getVersion(); double getVersion();
double getTime(); double getTime();
std::vector<double> getQTarget(); std::vector<double> getQTarget();
int getDigitalInputBits(); int getDigitalInputBits();
int getDigitalOutputBits(); int getDigitalOutputBits();
char getAnalogInputRange0(); char getAnalogInputRange0();
char getAnalogInputRange1(); char getAnalogInputRange1();
double getAnalogInput0(); double getAnalogInput0();
double getAnalogInput1(); double getAnalogInput1();
char getAnalogOutputDomain0(); char getAnalogOutputDomain0();
char getAnalogOutputDomain1(); char getAnalogOutputDomain1();
double getAnalogOutput0(); double getAnalogOutput0();
double getAnalogOutput1(); double getAnalogOutput1();
std::vector<double> getVActual(); std::vector<double> getVActual();
float getMasterBoardTemperature(); float getMasterBoardTemperature();
float getRobotVoltage48V(); float getRobotVoltage48V();
float getRobotCurrent(); float getRobotCurrent();
float getMasterIOCurrent(); float getMasterIOCurrent();
unsigned char getSafetyMode(); unsigned char getSafetyMode();
unsigned char getInReducedMode(); unsigned char getInReducedMode();
char getEuromap67InterfaceInstalled(); char getEuromap67InterfaceInstalled();
int getEuromapInputBits(); int getEuromapInputBits();
int getEuromapOutputBits(); int getEuromapOutputBits();
float getEuromapVoltage(); float getEuromapVoltage();
float getEuromapCurrent(); float getEuromapCurrent();
bool isRobotConnected(); bool isRobotConnected();
bool isRealRobotEnabled(); bool isRealRobotEnabled();
bool isPowerOnRobot(); bool isPowerOnRobot();
bool isEmergencyStopped(); bool isEmergencyStopped();
bool isProtectiveStopped(); bool isProtectiveStopped();
bool isProgramRunning(); bool isProgramRunning();
bool isProgramPaused(); bool isProgramPaused();
unsigned char getRobotMode(); unsigned char getRobotMode();
bool isReady(); bool isReady();
void setDisconnected(); void setDisconnected();
bool getNewDataAvailable(); bool getNewDataAvailable();
void finishedReading(); void finishedReading();
void unpack(uint8_t * buf, unsigned int buf_length); void unpack(uint8_t* buf, unsigned int buf_length);
void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len); void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset, void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
uint32_t len); uint32_t len);
void unpackRobotState(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 unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
void unpackRobotMode(uint8_t * buf, unsigned int offset); void unpackRobotMode(uint8_t* buf, unsigned int offset);
}; };
#endif /* ROBOT_STATE_H_ */ #endif /* ROBOT_STATE_H_ */

156
include/ur_modern_driver/robot_state_RT.h
View File

@@ -19,98 +19,98 @@
#ifndef ROBOT_STATE_RT_H_ #ifndef ROBOT_STATE_RT_H_
#define ROBOT_STATE_RT_H_ #define ROBOT_STATE_RT_H_
#include <condition_variable>
#include <inttypes.h> #include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex> #include <mutex>
#include <netinet/in.h> #include <netinet/in.h>
#include <condition_variable> #include <stdlib.h>
#include <string.h>
#include <vector>
class RobotStateRT { class RobotStateRT {
private: private:
double version_; //protocol version double version_; //protocol version
double time_; //Time elapsed since the controller was started double time_; //Time elapsed since the controller was started
std::vector<double> q_target_; //Target joint positions std::vector<double> q_target_; //Target joint positions
std::vector<double> qd_target_; //Target joint velocities std::vector<double> qd_target_; //Target joint velocities
std::vector<double> qdd_target_; //Target joint accelerations std::vector<double> qdd_target_; //Target joint accelerations
std::vector<double> i_target_; //Target joint currents std::vector<double> i_target_; //Target joint currents
std::vector<double> m_target_; //Target joint moments (torques) std::vector<double> m_target_; //Target joint moments (torques)
std::vector<double> q_actual_; //Actual joint positions std::vector<double> q_actual_; //Actual joint positions
std::vector<double> qd_actual_; //Actual joint velocities std::vector<double> qd_actual_; //Actual joint velocities
std::vector<double> i_actual_; //Actual joint currents std::vector<double> i_actual_; //Actual joint currents
std::vector<double> i_control_; //Joint control 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> 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_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
std::vector<double> tcp_force_; //Generalised forces in the TC 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> 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<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<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 std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
double controller_timer_; //Controller realtime thread execution time double controller_timer_; //Controller realtime thread execution time
double robot_mode_; //Robot mode double robot_mode_; //Robot mode
std::vector<double> joint_modes_; //Joint control modes std::vector<double> joint_modes_; //Joint control modes
double safety_mode_; //Safety mode double safety_mode_; //Safety mode
std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7) 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 speed_scaling_; //Speed scaling of the trajectory limiter
double linear_momentum_norm_; //Norm of Cartesian linear momentum double linear_momentum_norm_; //Norm of Cartesian linear momentum
double v_main_; //Masterboard: Main voltage double v_main_; //Masterboard: Main voltage
double v_robot_; //Matorborad: Robot voltage (48V) double v_robot_; //Matorborad: Robot voltage (48V)
double i_robot_; //Masterboard: Robot current double i_robot_; //Masterboard: Robot current
std::vector<double> v_actual_; //Actual joint voltages 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 std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool data_published_; //to avoid spurious wakes bool data_published_; //to avoid spurious wakes
bool controller_updated_; //to avoid spurious wakes bool controller_updated_; //to avoid spurious wakes
std::vector<double> unpackVector(uint8_t * buf, int start_index, std::vector<double> unpackVector(uint8_t* buf, int start_index,
int nr_of_vals); int nr_of_vals);
std::vector<bool> unpackDigitalInputBits(int64_t data); std::vector<bool> unpackDigitalInputBits(int64_t data);
double ntohd(uint64_t nf); double ntohd(uint64_t nf);
public: public:
RobotStateRT(std::condition_variable& msg_cond); RobotStateRT(std::condition_variable& msg_cond);
~RobotStateRT(); ~RobotStateRT();
double getVersion(); double getVersion();
double getTime(); double getTime();
std::vector<double> getQTarget(); std::vector<double> getQTarget();
std::vector<double> getQdTarget(); std::vector<double> getQdTarget();
std::vector<double> getQddTarget(); std::vector<double> getQddTarget();
std::vector<double> getITarget(); std::vector<double> getITarget();
std::vector<double> getMTarget(); std::vector<double> getMTarget();
std::vector<double> getQActual(); std::vector<double> getQActual();
std::vector<double> getQdActual(); std::vector<double> getQdActual();
std::vector<double> getIActual(); std::vector<double> getIActual();
std::vector<double> getIControl(); std::vector<double> getIControl();
std::vector<double> getToolVectorActual(); std::vector<double> getToolVectorActual();
std::vector<double> getTcpSpeedActual(); std::vector<double> getTcpSpeedActual();
std::vector<double> getTcpForce(); std::vector<double> getTcpForce();
std::vector<double> getToolVectorTarget(); std::vector<double> getToolVectorTarget();
std::vector<double> getTcpSpeedTarget(); std::vector<double> getTcpSpeedTarget();
std::vector<bool> getDigitalInputBits(); std::vector<bool> getDigitalInputBits();
std::vector<double> getMotorTemperatures(); std::vector<double> getMotorTemperatures();
double getControllerTimer(); double getControllerTimer();
double getRobotMode(); double getRobotMode();
std::vector<double> getJointModes(); std::vector<double> getJointModes();
double getSafety_mode(); double getSafety_mode();
std::vector<double> getToolAccelerometerValues(); std::vector<double> getToolAccelerometerValues();
double getSpeedScaling(); double getSpeedScaling();
double getLinearMomentumNorm(); double getLinearMomentumNorm();
double getVMain(); double getVMain();
double getVRobot(); double getVRobot();
double getIRobot(); double getIRobot();
void setVersion(double ver); void setVersion(double ver);
void setDataPublished(); void setDataPublished();
bool getDataPublished(); bool getDataPublished();
bool getControllerUpdated(); bool getControllerUpdated();
void setControllerUpdated(); void setControllerUpdated();
std::vector<double> getVActual(); std::vector<double> getVActual();
void unpack(uint8_t * buf); void unpack(uint8_t* buf);
}; };
#endif /* ROBOT_STATE_RT_H_ */ #endif /* ROBOT_STATE_RT_H_ */

1
include/ur_modern_driver/ros/converter.h
View File

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

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

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

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

@@ -1,46 +1,47 @@
#pragma once #pragma once
#include "ur_modern_driver/pipeline.h" #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/master_board.h"
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/rt_state.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> { class URRTPacketConsumer : public IConsumer<RTPacket> {
public: public:
virtual bool consume(unique_ptr<RTPacket> packet) { virtual bool consume(unique_ptr<RTPacket> packet)
{
return packet->consume_with(*this); return packet->consume_with(*this);
} }
virtual bool consume(RTState_V1_6__7 &state) = 0; virtual bool consume(RTState_V1_6__7& state) = 0;
virtual bool consume(RTState_V1_8 &state) = 0; virtual bool consume(RTState_V1_8& state) = 0;
virtual bool consume(RTState_V3_0__1 &state) = 0; virtual bool consume(RTState_V3_0__1& state) = 0;
virtual bool consume(RTState_V3_2__3 &state) = 0; virtual bool consume(RTState_V3_2__3& state) = 0;
}; };
class URStatePacketConsumer : public IConsumer<StatePacket> { class URStatePacketConsumer : public IConsumer<StatePacket> {
public: public:
virtual bool consume(unique_ptr<StatePacket> packet) { virtual bool consume(unique_ptr<StatePacket> packet)
{
return packet->consume_with(*this); return packet->consume_with(*this);
} }
virtual bool consume(MasterBoardData_V1_X &data) = 0; virtual bool consume(MasterBoardData_V1_X& data) = 0;
virtual bool consume(MasterBoardData_V3_0__1 &data) = 0; virtual bool consume(MasterBoardData_V3_0__1& data) = 0;
virtual bool consume(MasterBoardData_V3_2 &data) = 0; virtual bool consume(MasterBoardData_V3_2& data) = 0;
virtual bool consume(RobotModeData_V1_X &data) = 0; virtual bool consume(RobotModeData_V1_X& data) = 0;
virtual bool consume(RobotModeData_V3_0__1 &data) = 0; virtual bool consume(RobotModeData_V3_0__1& data) = 0;
virtual bool consume(RobotModeData_V3_2 &data) = 0; virtual bool consume(RobotModeData_V3_2& data) = 0;
}; };
class URMessagePacketConsumer : public IConsumer<MessagePacket> { class URMessagePacketConsumer : public IConsumer<MessagePacket> {
public: public:
virtual bool consume(unique_ptr<MessagePacket> packet) { virtual bool consume(unique_ptr<MessagePacket> packet)
{
return packet->consume_with(*this); 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 #pragma once
#include <cstdlib>
#include "ur_modern_driver/ur/stream.h"
#include "ur_modern_driver/ur/consumer.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/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 { class URFactory : private URMessagePacketConsumer {
private: private:
@@ -18,7 +17,8 @@ private:
uint8_t _major_version; uint8_t _major_version;
uint8_t _minor_version; uint8_t _minor_version;
bool consume(VersionMessage &vm) { bool consume(VersionMessage& vm)
{
LOG_INFO("Got VersionMessage:"); LOG_INFO("Got VersionMessage:");
LOG_INFO("project name: %s", vm.project_name.c_str()); LOG_INFO("project name: %s", vm.project_name.c_str());
LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version); LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
@@ -30,27 +30,29 @@ private:
return true; return true;
} }
void setup_consumer() { } void setup_consumer() {}
void teardown_consumer() { } void teardown_consumer() {}
void stop_consumer() { } void stop_consumer() {}
public: public:
URFactory(std::string &host) : _stream(host, 30001) { URFactory(std::string& host)
: _stream(host, 30001)
{
URProducer<MessagePacket> p(_stream, _parser); URProducer<MessagePacket> p(_stream, _parser);
std::vector<unique_ptr<MessagePacket>> results; std::vector<unique_ptr<MessagePacket> > results;
p.setup_producer(); 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!"); LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE); std::exit(EXIT_FAILURE);
} }
for(auto const& p : results) { for (auto const& p : results) {
p->consume_with(*this); 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!"); LOG_FATAL("No version message received, init failed!");
std::exit(EXIT_FAILURE); std::exit(EXIT_FAILURE);
} }
@@ -58,28 +60,30 @@ public:
p.teardown_producer(); p.teardown_producer();
} }
std::unique_ptr<URParser<StatePacket>> get_state_parser() { std::unique_ptr<URParser<StatePacket> > get_state_parser()
if(_major_version == 1) { {
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X); if (_major_version == 1) {
return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V1_X);
} else { } else {
if(_minor_version < 3) if (_minor_version < 3)
return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1); return std::unique_ptr<URParser<StatePacket> >(new URStateParser_V3_0__1);
else 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() { std::unique_ptr<URParser<RTPacket> > get_rt_parser()
if(_major_version == 1) { {
if(_minor_version < 8) if (_major_version == 1) {
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7); if (_minor_version < 8)
return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_6__7);
else else
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8); return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V1_8);
} else { } else {
if(_minor_version < 3) if (_minor_version < 3)
return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1); return std::unique_ptr<URParser<RTPacket> >(new URRTStateParser_V3_0__1);
else 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 #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 <cstddef>
#include <inttypes.h> #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 { class SharedMasterBoardData {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
int8_t analog_input_range0; int8_t analog_input_range0;
int8_t analog_input_range1; int8_t analog_input_range1;
@@ -40,8 +39,8 @@ public:
class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket { class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer &consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
int16_t digital_input_bits; int16_t digital_input_bits;
int16_t digital_output_bits; int16_t digital_output_bits;
@@ -53,7 +52,6 @@ public:
int16_t euromap_voltage; int16_t euromap_voltage;
int16_t euromap_current; int16_t euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int16_t) * 2 + sizeof(int16_t) * 2
+ sizeof(uint8_t) * 2; + sizeof(uint8_t) * 2;
@@ -64,8 +62,8 @@ public:
class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket { class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer &consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
int32_t digital_input_bits; int32_t digital_input_bits;
int32_t digital_output_bits; int32_t digital_output_bits;
@@ -77,7 +75,6 @@ public:
float euromap_voltage; float euromap_voltage;
float euromap_current; float euromap_current;
static const size_t SIZE = SharedMasterBoardData::SIZE static const size_t SIZE = SharedMasterBoardData::SIZE
+ sizeof(int32_t) * 2 + sizeof(int32_t) * 2
+ sizeof(uint8_t) * 2 + sizeof(uint8_t) * 2
@@ -89,8 +86,8 @@ public:
class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 { class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer &consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
uint8_t operational_mode_selector_input; uint8_t operational_mode_selector_input;
uint8_t three_position_enabling_device_input; uint8_t three_position_enabling_device_input;

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

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

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

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

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

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

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

@@ -1,14 +1,14 @@
#pragma once #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 <cstddef>
#include <inttypes.h> #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 { class SharedRobotModeData {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
uint64_t timestamp; uint64_t timestamp;
bool physical_robot_connected; bool physical_robot_connected;
@@ -23,23 +23,22 @@ public:
enum class robot_mode_V1_X : uint8_t { enum class robot_mode_V1_X : uint8_t {
ROBOT_RUNNING_MODE = 0, ROBOT_RUNNING_MODE = 0,
ROBOT_FREEDRIVE_MODE = 1, ROBOT_FREEDRIVE_MODE = 1,
ROBOT_READY_MODE = 2, ROBOT_READY_MODE = 2,
ROBOT_INITIALIZING_MODE = 3, ROBOT_INITIALIZING_MODE = 3,
ROBOT_SECURITY_STOPPED_MODE = 4, ROBOT_SECURITY_STOPPED_MODE = 4,
ROBOT_EMERGENCY_STOPPED_MODE = 5, ROBOT_EMERGENCY_STOPPED_MODE = 5,
ROBOT_FATAL_ERROR_MODE = 6, ROBOT_FATAL_ERROR_MODE = 6,
ROBOT_NO_POWER_MODE = 7, ROBOT_NO_POWER_MODE = 7,
ROBOT_NOT_CONNECTED_MODE = 8, ROBOT_NOT_CONNECTED_MODE = 8,
ROBOT_SHUTDOWN_MODE = 9, ROBOT_SHUTDOWN_MODE = 9,
ROBOT_SAFEGUARD_STOP_MODE = 10 ROBOT_SAFEGUARD_STOP_MODE = 10
}; };
class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket { class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer &consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
bool security_stopped; bool security_stopped;
robot_mode_V1_X robot_mode; robot_mode_V1_X robot_mode;
@@ -55,14 +54,14 @@ public:
enum class robot_mode_V3_X : uint8_t { enum class robot_mode_V3_X : uint8_t {
DISCONNECTED = 0, DISCONNECTED = 0,
CONFIRM_SAFETY = 1, CONFIRM_SAFETY = 1,
BOOTING = 2, BOOTING = 2,
POWER_OFF = 3, POWER_OFF = 3,
POWER_ON = 4, POWER_ON = 4,
IDLE = 5, IDLE = 5,
BACKDRIVE = 6, BACKDRIVE = 6,
RUNNING = 7, RUNNING = 7,
UPDATING_FIRMWARE = 8 UPDATING_FIRMWARE = 8
}; };
enum class robot_control_mode_V3_X : uint8_t { 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 { class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer &consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
bool protective_stopped; bool protective_stopped;
@@ -98,9 +96,8 @@ public:
class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
public: public:
virtual bool parse_with(BinParser &bp); virtual bool parse_with(BinParser& bp);
virtual bool consume_with(URStatePacketConsumer &consumer); virtual bool consume_with(URStatePacketConsumer& consumer);
double target_speed_fraction_limit; double target_speed_fraction_limit;

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

@@ -1,19 +1,19 @@
#pragma once #pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h" #include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/rt_state.h" #include "ur_modern_driver/ur/rt_state.h"
#include <vector>
template <typename T> template <typename T>
class URRTStateParser : public URParser<RTPacket> { class URRTStateParser : public URParser<RTPacket> {
public: 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>(); 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"); LOG_ERROR("Buffer len shorter than expected packet length");
return false; return false;
} }
@@ -21,7 +21,7 @@ public:
bp.parse(packet_size); //consumes the peeked data bp.parse(packet_size); //consumes the peeked data
std::unique_ptr<RTPacket> packet(new T); std::unique_ptr<RTPacket> packet(new T);
if(!packet->parse_with(bp)) if (!packet->parse_with(bp))
return false; return false;
results.push_back(std::move(packet)); results.push_back(std::move(packet));

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

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

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

@@ -1,26 +1,26 @@
#pragma once #pragma once
#include <cstddef>
#include <inttypes.h>
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include <cstddef>
#include <inttypes.h>
enum class package_type : uint8_t { enum class package_type : uint8_t {
ROBOT_MODE_DATA = 0, ROBOT_MODE_DATA = 0,
JOINT_DATA = 1, JOINT_DATA = 1,
TOOL_DATA = 2, TOOL_DATA = 2,
MASTERBOARD_DATA = 3, MASTERBOARD_DATA = 3,
CARTESIAN_INFO = 4, CARTESIAN_INFO = 4,
KINEMATICS_INFO = 5, KINEMATICS_INFO = 5,
CONFIGURATION_DATA = 6, CONFIGURATION_DATA = 6,
FORCE_MODE_DATA = 7, FORCE_MODE_DATA = 7,
ADDITIONAL_INFO = 8, ADDITIONAL_INFO = 8,
CALIBRATION_DATA = 9 CALIBRATION_DATA = 9
}; };
enum class message_type : uint8_t { enum class message_type : uint8_t {
ROBOT_STATE = 16, ROBOT_STATE = 16,
ROBOT_MESSAGE = 20, ROBOT_MESSAGE = 20,
PROGRAM_STATE_MESSAGE = 25 PROGRAM_STATE_MESSAGE = 25
}; };
@@ -29,6 +29,6 @@ class URStatePacketConsumer;
class StatePacket { class StatePacket {
public: public:
virtual bool parse_with(BinParser &bp) = 0; virtual bool parse_with(BinParser& bp) = 0;
virtual bool consume_with(URStatePacketConsumer &consumer) = 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 #pragma once
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.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/master_board.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/robot_mode.h" #include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/state.h"
#include <vector>
template <typename RMD, typename MBD> template <typename RMD, typename MBD>
class URStateParser : public URParser<StatePacket> { class URStateParser : public URParser<StatePacket> {
private: private:
StatePacket* from_type(package_type type) { StatePacket* from_type(package_type type)
switch(type) { {
case package_type::ROBOT_MODE_DATA: switch (type) {
return new RMD; case package_type::ROBOT_MODE_DATA:
case package_type::MASTERBOARD_DATA: return new RMD;
return new MBD; case package_type::MASTERBOARD_DATA:
default: return new MBD;
return nullptr; default:
return nullptr;
} }
} }
public: 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; int32_t packet_size;
message_type type; message_type type;
bp.parse(packet_size); bp.parse(packet_size);
bp.parse(type); 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)); LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
return false; return false;
} }
while(!bp.empty()) { while (!bp.empty()) {
if(!bp.check_size(sizeof(uint32_t))){ if (!bp.check_size(sizeof(uint32_t))) {
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false; return false;
} }
uint32_t sub_size = bp.peek<uint32_t>(); 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); LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false; return false;
} }
@@ -54,20 +55,20 @@ public:
std::unique_ptr<StatePacket> packet(from_type(type)); std::unique_ptr<StatePacket> packet(from_type(type));
if(packet == nullptr) { if (packet == nullptr) {
sbp.consume(); sbp.consume();
LOG_INFO("Skipping sub-packet of type %d", type); LOG_INFO("Skipping sub-packet of type %d", type);
continue; continue;
} }
if(!packet->parse_with(sbp)) { if (!packet->parse_with(sbp)) {
LOG_ERROR("Sub-package parsing of type %d failed!", type); LOG_ERROR("Sub-package parsing of type %d failed!", type);
return false; return false;
} }
results.push_back(std::move(packet)); results.push_back(std::move(packet));
if(!sbp.empty()) { if (!sbp.empty()) {
LOG_ERROR("Sub-package was not parsed completely!"); LOG_ERROR("Sub-package was not parsed completely!");
return false; return false;
} }
@@ -77,6 +78,6 @@ public:
} }
}; };
typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X> URStateParser_V1_X; 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_0__1, MasterBoardData_V3_0__1> URStateParser_V3_0__1;
typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2> URStateParser_V3_2; 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 #pragma once
#include <string>
#include <atomic> #include <atomic>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h> #include <netdb.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
/// Encapsulates a TCP socket /// Encapsulates a TCP socket
class URStream { class URStream {
@@ -16,19 +16,22 @@ private:
std::atomic<bool> _stopping; std::atomic<bool> _stopping;
public: public:
URStream(std::string &host, int port) URStream(std::string& host, int port)
: _host(host), : _host(host)
_port(port), , _port(port)
_initialized(false), , _initialized(false)
_stopping(false) {} , _stopping(false)
{
}
~URStream() { ~URStream()
{
disconnect(); disconnect();
} }
bool connect(); bool connect();
void disconnect(); void disconnect();
ssize_t send(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); 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_ #ifndef UR_COMMUNICATION_H_
#define UR_COMMUNICATION_H_ #define UR_COMMUNICATION_H_
#include "robot_state.h"
#include "do_output.h" #include "do_output.h"
#include <vector> #include "robot_state.h"
#include <stdlib.h> #include <chrono>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable> #include <condition_variable>
#include <sys/types.h> #include <fcntl.h>
#include <sys/socket.h> #include <iostream>
#include <mutex>
#include <netdb.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <netinet/tcp.h> #include <netinet/tcp.h>
#include <netdb.h> #include <stdio.h>
#include <iostream> #include <stdlib.h>
#include <unistd.h> #include <string.h>
#include <chrono> #include <sys/socket.h>
#include <fcntl.h> #include <sys/time.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
class UrCommunication { class UrCommunication {
private: private:
int pri_sockfd_, sec_sockfd_; int pri_sockfd_, sec_sockfd_;
struct sockaddr_in pri_serv_addr_, sec_serv_addr_; struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
struct hostent *server_; struct hostent* server_;
bool keepalive_; bool keepalive_;
std::thread comThread_; std::thread comThread_;
int flag_; int flag_;
void run(); void run();
public: public:
bool connected_; bool connected_;
RobotState* robot_state_; RobotState* robot_state_;
UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start();
void halt();
UrCommunication(std::condition_variable& msg_cond, std::string host);
bool start();
void halt();
}; };
#endif /* UR_COMMUNICATION_H_ */ #endif /* UR_COMMUNICATION_H_ */

118
include/ur_modern_driver/ur_driver.h
View File

@@ -19,83 +19,81 @@
#ifndef UR_DRIVER_H_ #ifndef UR_DRIVER_H_
#define 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 "do_output.h"
#include <vector> #include "ur_communication.h"
#include "ur_realtime_communication.h"
#include <condition_variable>
#include <math.h> #include <math.h>
#include <string> #include <mutex>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>
#include <chrono> #include <chrono>
class UrDriver { class UrDriver {
private: private:
double maximum_time_step_; double maximum_time_step_;
double minimum_payload_; double minimum_payload_;
double maximum_payload_; double maximum_payload_;
std::vector<std::string> joint_names_; std::vector<std::string> joint_names_;
std::string ip_addr_; std::string ip_addr_;
const int MULT_JOINTSTATE_ = 1000000; const int MULT_JOINTSTATE_ = 1000000;
const int MULT_TIME_ = 1000000; const int MULT_TIME_ = 1000000;
const unsigned int REVERSE_PORT_; const unsigned int REVERSE_PORT_;
int incoming_sockfd_; int incoming_sockfd_;
int new_sockfd_; int new_sockfd_;
bool reverse_connected_; bool reverse_connected_;
double servoj_time_; double servoj_time_;
bool executing_traj_; bool executing_traj_;
double firmware_version_; double firmware_version_;
double servoj_lookahead_time_; double servoj_lookahead_time_;
double servoj_gain_; double servoj_gain_;
public: public:
UrRealtimeCommunication* rt_interface_; UrRealtimeCommunication* rt_interface_;
UrCommunication* sec_interface_; UrCommunication* sec_interface_;
UrDriver(std::condition_variable& rt_msg_cond, UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host, std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 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.,
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.);
double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.); bool start();
bool start(); void halt();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4, void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.); double q5, double acc = 100.);
bool doTraj(std::vector<double> inp_timestamps, bool doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions, std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities); std::vector<std::vector<double> > inp_velocities);
void servoj(std::vector<double> positions, int keepalive = 1); void servoj(std::vector<double> positions, int keepalive = 1);
void stopTraj(); void stopTraj();
bool uploadProg(); bool uploadProg();
bool openServo(); bool openServo();
void closeServo(std::vector<double> positions); void closeServo(std::vector<double> positions);
std::vector<double> interp_cubic(double t, double T, std::vector<double> interp_cubic(double t, double T,
std::vector<double> p0_pos, std::vector<double> p1_pos, std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel); std::vector<double> p0_vel, std::vector<double> p1_vel);
std::vector<std::string> getJointNames(); std::vector<std::string> getJointNames();
void setJointNames(std::vector<std::string> jn); void setJointNames(std::vector<std::string> jn);
void setToolVoltage(unsigned int v); void setToolVoltage(unsigned int v);
void setFlag(unsigned int n, bool b); void setFlag(unsigned int n, bool b);
void setDigitalOut(unsigned int n, bool b); void setDigitalOut(unsigned int n, bool b);
void setAnalogOut(unsigned int n, double f); void setAnalogOut(unsigned int n, double f);
bool setPayload(double m); bool setPayload(double m);
void setMinPayload(double m);
void setMaxPayload(double m);
void setServojTime(double t);
void setServojLookahead(double t);
void setServojGain(double g);
void setMinPayload(double m);
void setMaxPayload(double m);
void setServojTime(double t);
void setServojLookahead(double t);
void setServojGain(double g);
}; };
#endif /* UR_DRIVER_H_ */ #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 #ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
#define 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 "do_output.h"
#include "ur_driver.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 { namespace ros_control_ur {
@@ -76,64 +76,61 @@ static const double POSITION_STEP_FACTOR = 1;
static const double VELOCITY_STEP_FACTOR = 1; static const double VELOCITY_STEP_FACTOR = 1;
/// \brief Hardware interface for a robot /// \brief Hardware interface for a robot
class UrHardwareInterface: public hardware_interface::RobotHW { class UrHardwareInterface : public hardware_interface::RobotHW {
public: public:
/**
/**
* \brief Constructor * \brief Constructor
* \param nh - Node handle for topics. * \param nh - Node handle for topics.
*/ */
UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
/// \brief Initialize the hardware interface /// \brief Initialize the hardware interface
virtual void init(); virtual void init();
/// \brief Read the state from the robot hardware. /// \brief Read the state from the robot hardware.
virtual void read(); virtual void read();
/// \brief write the command to the robot hardware. /// \brief write the command to the robot hardware.
virtual void write(); virtual void write();
void setMaxVelChange(double inp); void setMaxVelChange(double inp);
bool canSwitch( bool canSwitch(
const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo> &stop_list) const; const std::list<hardware_interface::ControllerInfo>& stop_list) const;
void doSwitch(const std::list<hardware_interface::ControllerInfo>&start_list, void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>&stop_list); const std::list<hardware_interface::ControllerInfo>& stop_list);
protected: 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 // Interfaces
ros::NodeHandle nh_; 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 double max_vel_change_;
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_;
// Robot API
UrDriver* robot_;
}; };
// class // class
}// namespace } // namespace
#endif #endif

80
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -19,58 +19,56 @@
#ifndef UR_REALTIME_COMMUNICATION_H_ #ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_ #define UR_REALTIME_COMMUNICATION_H_
#include "robot_state_RT.h"
#include "do_output.h" #include "do_output.h"
#include <vector> #include "robot_state_RT.h"
#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 <arpa/inet.h> #include <arpa/inet.h>
#include <condition_variable>
#include <errno.h> #include <errno.h>
#include <fcntl.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 <sys/types.h>
#include <thread>
#include <unistd.h>
#include <vector>
class UrRealtimeCommunication { class UrRealtimeCommunication {
private: private:
unsigned int safety_count_max_; unsigned int safety_count_max_;
int sockfd_; int sockfd_;
struct sockaddr_in serv_addr_; struct sockaddr_in serv_addr_;
struct hostent *server_; struct hostent* server_;
std::string local_ip_; std::string local_ip_;
bool keepalive_; bool keepalive_;
std::thread comThread_; std::thread comThread_;
int flag_; int flag_;
std::recursive_mutex command_string_lock_; std::recursive_mutex command_string_lock_;
std::string command_; std::string command_;
unsigned int safety_count_; unsigned int safety_count_;
void run(); void run();
public: public:
bool connected_; bool connected_;
RobotStateRT* robot_state_; RobotStateRT* robot_state_;
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max = 12);
bool start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp);
std::string getLocalIp();
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max = 12);
bool start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp);
std::string getLocalIp();
}; };
#endif /* UR_REALTIME_COMMUNICATION_H_ */ #endif /* UR_REALTIME_COMMUNICATION_H_ */

39
src/do_output.cpp
View File

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

671
src/robot_state.cpp
View File

@@ -18,370 +18,391 @@
#include "ur_modern_driver/robot_state.h" #include "ur_modern_driver/robot_state.h"
RobotState::RobotState(std::condition_variable& msg_cond) { RobotState::RobotState(std::condition_variable& msg_cond)
version_msg_.major_version = 0; {
version_msg_.minor_version = 0; version_msg_.major_version = 0;
new_data_available_ = false; version_msg_.minor_version = 0;
pMsg_cond_ = &msg_cond; new_data_available_ = false;
RobotState::setDisconnected(); pMsg_cond_ = &msg_cond;
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING; RobotState::setDisconnected();
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
} }
double RobotState::ntohd(uint64_t nf) { double RobotState::ntohd(uint64_t nf)
double x; {
nf = be64toh(nf); double x;
memcpy(&x, &nf, sizeof(x)); nf = be64toh(nf);
return x; memcpy(&x, &nf, sizeof(x));
return x;
} }
void RobotState::unpack(uint8_t* buf, unsigned int buf_length) { 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; /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
while (buf_length > offset) { unsigned int offset = 0;
int len; while (buf_length > offset) {
unsigned char message_type; int len;
memcpy(&len, &buf[offset], sizeof(len)); unsigned char message_type;
len = ntohl(len); memcpy(&len, &buf[offset], sizeof(len));
if (len + offset > buf_length) { len = ntohl(len);
return; if (len + offset > buf_length) {
} return;
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); }
switch (message_type) { memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
case messageType::ROBOT_MESSAGE: switch (message_type) {
RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType case messageType::ROBOT_MESSAGE:
break; RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
case messageType::ROBOT_STATE: break;
RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType case messageType::ROBOT_STATE:
break; RobotState::unpackRobotState(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
case messageType::PROGRAM_STATE_MESSAGE: break;
//Don't do anything atm... case messageType::PROGRAM_STATE_MESSAGE:
default: //Don't do anything atm...
break; default:
} break;
offset += len; }
offset += len;
} }
return; return;
} }
void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset, void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset,
uint32_t len) { uint32_t len)
offset += 5; {
uint64_t timestamp; offset += 5;
int8_t source, robot_message_type; uint64_t timestamp;
memcpy(&timestamp, &buf[offset], sizeof(timestamp)); int8_t source, robot_message_type;
offset += sizeof(timestamp); memcpy(&timestamp, &buf[offset], sizeof(timestamp));
memcpy(&source, &buf[offset], sizeof(source)); offset += sizeof(timestamp);
offset += sizeof(source); memcpy(&source, &buf[offset], sizeof(source));
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type)); offset += sizeof(source);
offset += sizeof(robot_message_type); memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
switch (robot_message_type) { offset += sizeof(robot_message_type);
case robotMessageType::ROBOT_MESSAGE_VERSION: switch (robot_message_type) {
val_lock_.lock(); case robotMessageType::ROBOT_MESSAGE_VERSION:
version_msg_.timestamp = timestamp; val_lock_.lock();
version_msg_.source = source; version_msg_.timestamp = timestamp;
version_msg_.robot_message_type = robot_message_type; version_msg_.source = source;
RobotState::unpackRobotMessageVersion(buf, offset, len); version_msg_.robot_message_type = robot_message_type;
val_lock_.unlock(); RobotState::unpackRobotMessageVersion(buf, offset, len);
break; val_lock_.unlock();
default: break;
break; default:
} break;
}
} }
void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset, void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset,
uint32_t len) { uint32_t len)
offset += 5; {
while (offset < len) { offset += 5;
int32_t length; while (offset < len) {
uint8_t package_type; int32_t length;
memcpy(&length, &buf[offset], sizeof(length)); uint8_t package_type;
length = ntohl(length); memcpy(&length, &buf[offset], sizeof(length));
memcpy(&package_type, &buf[offset + sizeof(length)], length = ntohl(length);
sizeof(package_type)); memcpy(&package_type, &buf[offset + sizeof(length)],
switch (package_type) { sizeof(package_type));
case packageType::ROBOT_MODE_DATA: switch (package_type) {
val_lock_.lock(); case packageType::ROBOT_MODE_DATA:
RobotState::unpackRobotMode(buf, offset + 5); val_lock_.lock();
val_lock_.unlock(); RobotState::unpackRobotMode(buf, offset + 5);
break; 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();
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, void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
uint32_t len) { uint32_t len)
memcpy(&version_msg_.project_name_size, &buf[offset], {
sizeof(version_msg_.project_name_size)); memcpy(&version_msg_.project_name_size, &buf[offset],
offset += sizeof(version_msg_.project_name_size); sizeof(version_msg_.project_name_size));
memcpy(&version_msg_.project_name, &buf[offset], offset += sizeof(version_msg_.project_name_size);
sizeof(char) * version_msg_.project_name_size); memcpy(&version_msg_.project_name, &buf[offset],
offset += version_msg_.project_name_size; sizeof(char) * version_msg_.project_name_size);
version_msg_.project_name[version_msg_.project_name_size] = '\0'; offset += version_msg_.project_name_size;
memcpy(&version_msg_.major_version, &buf[offset], version_msg_.project_name[version_msg_.project_name_size] = '\0';
sizeof(version_msg_.major_version)); memcpy(&version_msg_.major_version, &buf[offset],
offset += sizeof(version_msg_.major_version); sizeof(version_msg_.major_version));
memcpy(&version_msg_.minor_version, &buf[offset], offset += sizeof(version_msg_.major_version);
sizeof(version_msg_.minor_version)); memcpy(&version_msg_.minor_version, &buf[offset],
offset += sizeof(version_msg_.minor_version); sizeof(version_msg_.minor_version));
memcpy(&version_msg_.svn_revision, &buf[offset], offset += sizeof(version_msg_.minor_version);
sizeof(version_msg_.svn_revision)); memcpy(&version_msg_.svn_revision, &buf[offset],
offset += sizeof(version_msg_.svn_revision); sizeof(version_msg_.svn_revision));
version_msg_.svn_revision = ntohl(version_msg_.svn_revision); offset += sizeof(version_msg_.svn_revision);
memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset); version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
version_msg_.build_date[len - offset] = '\0'; memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
if (version_msg_.major_version < 2) { version_msg_.build_date[len - offset] = '\0';
robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE; if (version_msg_.major_version < 2) {
} robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
}
} }
void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) { void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp)); {
offset += sizeof(robot_mode_.timestamp); memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
uint8_t tmp; offset += sizeof(robot_mode_.timestamp);
memcpy(&tmp, &buf[offset], sizeof(tmp)); uint8_t tmp;
if (tmp > 0) memcpy(&tmp, &buf[offset], sizeof(tmp));
robot_mode_.isRobotConnected = true; if (tmp > 0)
else robot_mode_.isRobotConnected = true;
robot_mode_.isRobotConnected = false; else
offset += sizeof(tmp); robot_mode_.isRobotConnected = false;
memcpy(&tmp, &buf[offset], sizeof(tmp)); offset += sizeof(tmp);
if (tmp > 0) memcpy(&tmp, &buf[offset], sizeof(tmp));
robot_mode_.isRealRobotEnabled = true; if (tmp > 0)
else robot_mode_.isRealRobotEnabled = true;
robot_mode_.isRealRobotEnabled = false; else
offset += sizeof(tmp); robot_mode_.isRealRobotEnabled = false;
memcpy(&tmp, &buf[offset], sizeof(tmp)); offset += sizeof(tmp);
//printf("PowerOnRobot: %d\n", tmp); memcpy(&tmp, &buf[offset], sizeof(tmp));
if (tmp > 0) //printf("PowerOnRobot: %d\n", tmp);
robot_mode_.isPowerOnRobot = true; if (tmp > 0)
else robot_mode_.isPowerOnRobot = true;
robot_mode_.isPowerOnRobot = false; else
offset += sizeof(tmp); robot_mode_.isPowerOnRobot = false;
memcpy(&tmp, &buf[offset], sizeof(tmp)); offset += sizeof(tmp);
if (tmp > 0) memcpy(&tmp, &buf[offset], sizeof(tmp));
robot_mode_.isEmergencyStopped = true; if (tmp > 0)
else robot_mode_.isEmergencyStopped = true;
robot_mode_.isEmergencyStopped = false; else
offset += sizeof(tmp); robot_mode_.isEmergencyStopped = false;
memcpy(&tmp, &buf[offset], sizeof(tmp)); offset += sizeof(tmp);
if (tmp > 0) memcpy(&tmp, &buf[offset], sizeof(tmp));
robot_mode_.isProtectiveStopped = true; if (tmp > 0)
else robot_mode_.isProtectiveStopped = true;
robot_mode_.isProtectiveStopped = false; else
offset += sizeof(tmp); robot_mode_.isProtectiveStopped = false;
memcpy(&tmp, &buf[offset], sizeof(tmp)); offset += sizeof(tmp);
if (tmp > 0) memcpy(&tmp, &buf[offset], sizeof(tmp));
robot_mode_.isProgramRunning = true; if (tmp > 0)
else robot_mode_.isProgramRunning = true;
robot_mode_.isProgramRunning = false; else
offset += sizeof(tmp); robot_mode_.isProgramRunning = false;
memcpy(&tmp, &buf[offset], sizeof(tmp)); offset += sizeof(tmp);
if (tmp > 0) memcpy(&tmp, &buf[offset], sizeof(tmp));
robot_mode_.isProgramPaused = true; if (tmp > 0)
else robot_mode_.isProgramPaused = true;
robot_mode_.isProgramPaused = false; else
offset += sizeof(tmp); robot_mode_.isProgramPaused = false;
memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode)); offset += sizeof(tmp);
offset += sizeof(robot_mode_.robotMode); memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
uint64_t temp; offset += sizeof(robot_mode_.robotMode);
if (RobotState::getVersion() > 2.) { uint64_t temp;
memcpy(&robot_mode_.controlMode, &buf[offset], if (RobotState::getVersion() > 2.) {
sizeof(robot_mode_.controlMode)); memcpy(&robot_mode_.controlMode, &buf[offset],
offset += sizeof(robot_mode_.controlMode); sizeof(robot_mode_.controlMode));
memcpy(&temp, &buf[offset], sizeof(temp)); offset += sizeof(robot_mode_.controlMode);
offset += sizeof(temp); memcpy(&temp, &buf[offset], sizeof(temp));
robot_mode_.targetSpeedFraction = RobotState::ntohd(temp); offset += sizeof(temp);
} robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp)); }
offset += sizeof(temp); memcpy(&temp, &buf[offset], sizeof(temp));
robot_mode_.speedScaling = RobotState::ntohd(temp); offset += sizeof(temp);
robot_mode_.speedScaling = RobotState::ntohd(temp);
} }
void RobotState::unpackRobotStateMasterboard(uint8_t * buf, void RobotState::unpackRobotStateMasterboard(uint8_t* buf,
unsigned int offset) { unsigned int offset)
if (RobotState::getVersion() < 3.0) { {
int16_t digital_input_bits, digital_output_bits; if (RobotState::getVersion() < 3.0) {
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits)); int16_t digital_input_bits, digital_output_bits;
offset += sizeof(digital_input_bits); memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); offset += sizeof(digital_input_bits);
offset += sizeof(digital_output_bits); memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
mb_data_.digitalInputBits = ntohs(digital_input_bits); offset += sizeof(digital_output_bits);
mb_data_.digitalOutputBits = ntohs(digital_output_bits); mb_data_.digitalInputBits = ntohs(digital_input_bits);
} else { mb_data_.digitalOutputBits = ntohs(digital_output_bits);
memcpy(&mb_data_.digitalInputBits, &buf[offset], } else {
sizeof(mb_data_.digitalInputBits)); memcpy(&mb_data_.digitalInputBits, &buf[offset],
offset += sizeof(mb_data_.digitalInputBits); sizeof(mb_data_.digitalInputBits));
mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits); offset += sizeof(mb_data_.digitalInputBits);
memcpy(&mb_data_.digitalOutputBits, &buf[offset], mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
sizeof(mb_data_.digitalOutputBits)); memcpy(&mb_data_.digitalOutputBits, &buf[offset],
offset += sizeof(mb_data_.digitalOutputBits); sizeof(mb_data_.digitalOutputBits));
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits); offset += sizeof(mb_data_.digitalOutputBits);
} mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
}
memcpy(&mb_data_.analogInputRange0, &buf[offset], memcpy(&mb_data_.analogInputRange0, &buf[offset],
sizeof(mb_data_.analogInputRange0)); sizeof(mb_data_.analogInputRange0));
offset += sizeof(mb_data_.analogInputRange0); offset += sizeof(mb_data_.analogInputRange0);
memcpy(&mb_data_.analogInputRange1, &buf[offset], memcpy(&mb_data_.analogInputRange1, &buf[offset],
sizeof(mb_data_.analogInputRange1)); sizeof(mb_data_.analogInputRange1));
offset += sizeof(mb_data_.analogInputRange1); offset += sizeof(mb_data_.analogInputRange1);
uint64_t temp; uint64_t temp;
memcpy(&temp, &buf[offset], sizeof(temp)); memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp); offset += sizeof(temp);
mb_data_.analogInput0 = RobotState::ntohd(temp); mb_data_.analogInput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp)); memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp); offset += sizeof(temp);
mb_data_.analogInput1 = RobotState::ntohd(temp); mb_data_.analogInput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.analogOutputDomain0, &buf[offset], memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
sizeof(mb_data_.analogOutputDomain0)); sizeof(mb_data_.analogOutputDomain0));
offset += sizeof(mb_data_.analogOutputDomain0); offset += sizeof(mb_data_.analogOutputDomain0);
memcpy(&mb_data_.analogOutputDomain1, &buf[offset], memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
sizeof(mb_data_.analogOutputDomain1)); sizeof(mb_data_.analogOutputDomain1));
offset += sizeof(mb_data_.analogOutputDomain1); offset += sizeof(mb_data_.analogOutputDomain1);
memcpy(&temp, &buf[offset], sizeof(temp)); memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp); offset += sizeof(temp);
mb_data_.analogOutput0 = RobotState::ntohd(temp); mb_data_.analogOutput0 = RobotState::ntohd(temp);
memcpy(&temp, &buf[offset], sizeof(temp)); memcpy(&temp, &buf[offset], sizeof(temp));
offset += sizeof(temp); offset += sizeof(temp);
mb_data_.analogOutput1 = RobotState::ntohd(temp); mb_data_.analogOutput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.masterBoardTemperature, &buf[offset], memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
sizeof(mb_data_.masterBoardTemperature)); sizeof(mb_data_.masterBoardTemperature));
offset += sizeof(mb_data_.masterBoardTemperature); offset += sizeof(mb_data_.masterBoardTemperature);
mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature); mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
memcpy(&mb_data_.robotVoltage48V, &buf[offset], memcpy(&mb_data_.robotVoltage48V, &buf[offset],
sizeof(mb_data_.robotVoltage48V)); sizeof(mb_data_.robotVoltage48V));
offset += sizeof(mb_data_.robotVoltage48V); offset += sizeof(mb_data_.robotVoltage48V);
mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V); mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
offset += sizeof(mb_data_.robotCurrent); offset += sizeof(mb_data_.robotCurrent);
mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent); mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
memcpy(&mb_data_.masterIOCurrent, &buf[offset], memcpy(&mb_data_.masterIOCurrent, &buf[offset],
sizeof(mb_data_.masterIOCurrent)); sizeof(mb_data_.masterIOCurrent));
offset += sizeof(mb_data_.masterIOCurrent); offset += sizeof(mb_data_.masterIOCurrent);
mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent); mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
offset += sizeof(mb_data_.safetyMode); offset += sizeof(mb_data_.safetyMode);
memcpy(&mb_data_.masterOnOffState, &buf[offset], memcpy(&mb_data_.masterOnOffState, &buf[offset],
sizeof(mb_data_.masterOnOffState)); sizeof(mb_data_.masterOnOffState));
offset += sizeof(mb_data_.masterOnOffState); offset += sizeof(mb_data_.masterOnOffState);
memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset],
sizeof(mb_data_.euromap67InterfaceInstalled)); sizeof(mb_data_.euromap67InterfaceInstalled));
offset += sizeof(mb_data_.euromap67InterfaceInstalled); offset += sizeof(mb_data_.euromap67InterfaceInstalled);
if (mb_data_.euromap67InterfaceInstalled != 0) { if (mb_data_.euromap67InterfaceInstalled != 0) {
memcpy(&mb_data_.euromapInputBits, &buf[offset], memcpy(&mb_data_.euromapInputBits, &buf[offset],
sizeof(mb_data_.euromapInputBits)); sizeof(mb_data_.euromapInputBits));
offset += sizeof(mb_data_.euromapInputBits); offset += sizeof(mb_data_.euromapInputBits);
mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits); mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
memcpy(&mb_data_.euromapOutputBits, &buf[offset], memcpy(&mb_data_.euromapOutputBits, &buf[offset],
sizeof(mb_data_.euromapOutputBits)); sizeof(mb_data_.euromapOutputBits));
offset += sizeof(mb_data_.euromapOutputBits); offset += sizeof(mb_data_.euromapOutputBits);
mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits); mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
if (RobotState::getVersion() < 3.0) { if (RobotState::getVersion() < 3.0) {
int16_t euromap_voltage, euromap_current; int16_t euromap_voltage, euromap_current;
memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
offset += sizeof(euromap_voltage); offset += sizeof(euromap_voltage);
memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
offset += sizeof(euromap_current); offset += sizeof(euromap_current);
mb_data_.euromapVoltage = ntohs(euromap_voltage); mb_data_.euromapVoltage = ntohs(euromap_voltage);
mb_data_.euromapCurrent = ntohs(euromap_current); mb_data_.euromapCurrent = ntohs(euromap_current);
} else { } else {
memcpy(&mb_data_.euromapVoltage, &buf[offset], memcpy(&mb_data_.euromapVoltage, &buf[offset],
sizeof(mb_data_.euromapVoltage)); sizeof(mb_data_.euromapVoltage));
offset += sizeof(mb_data_.euromapVoltage); offset += sizeof(mb_data_.euromapVoltage);
mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage); mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
memcpy(&mb_data_.euromapCurrent, &buf[offset], memcpy(&mb_data_.euromapCurrent, &buf[offset],
sizeof(mb_data_.euromapCurrent)); sizeof(mb_data_.euromapCurrent));
offset += sizeof(mb_data_.euromapCurrent); offset += sizeof(mb_data_.euromapCurrent);
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent); mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
} }
}
}
} }
double RobotState::getVersion() { double RobotState::getVersion()
double ver; {
val_lock_.lock(); double ver;
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version val_lock_.lock();
+ .0000001 * version_msg_.svn_revision; ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
val_lock_.unlock(); + .0000001 * version_msg_.svn_revision;
return ver; val_lock_.unlock();
return ver;
} }
void RobotState::finishedReading() { void RobotState::finishedReading()
new_data_available_ = false; {
new_data_available_ = false;
} }
bool RobotState::getNewDataAvailable() { bool RobotState::getNewDataAvailable()
return new_data_available_; {
return new_data_available_;
} }
int RobotState::getDigitalInputBits() { int RobotState::getDigitalInputBits()
return mb_data_.digitalInputBits; {
return mb_data_.digitalInputBits;
} }
int RobotState::getDigitalOutputBits() { int RobotState::getDigitalOutputBits()
return mb_data_.digitalOutputBits; {
return mb_data_.digitalOutputBits;
} }
double RobotState::getAnalogInput0() { double RobotState::getAnalogInput0()
return mb_data_.analogInput0; {
return mb_data_.analogInput0;
} }
double RobotState::getAnalogInput1() { double RobotState::getAnalogInput1()
return mb_data_.analogInput1; {
return mb_data_.analogInput1;
} }
double RobotState::getAnalogOutput0() { double RobotState::getAnalogOutput0()
return mb_data_.analogOutput0; {
return mb_data_.analogOutput0;
} }
double RobotState::getAnalogOutput1() { double RobotState::getAnalogOutput1()
return mb_data_.analogOutput1; {
return mb_data_.analogOutput1;
} }
bool RobotState::isRobotConnected() { bool RobotState::isRobotConnected()
return robot_mode_.isRobotConnected; {
return robot_mode_.isRobotConnected;
} }
bool RobotState::isRealRobotEnabled() { bool RobotState::isRealRobotEnabled()
return robot_mode_.isRealRobotEnabled; {
return robot_mode_.isRealRobotEnabled;
} }
bool RobotState::isPowerOnRobot() { bool RobotState::isPowerOnRobot()
return robot_mode_.isPowerOnRobot; {
return robot_mode_.isPowerOnRobot;
} }
bool RobotState::isEmergencyStopped() { bool RobotState::isEmergencyStopped()
return robot_mode_.isEmergencyStopped; {
return robot_mode_.isEmergencyStopped;
} }
bool RobotState::isProtectiveStopped() { bool RobotState::isProtectiveStopped()
return robot_mode_.isProtectiveStopped; {
return robot_mode_.isProtectiveStopped;
} }
bool RobotState::isProgramRunning() { bool RobotState::isProgramRunning()
return robot_mode_.isProgramRunning; {
return robot_mode_.isProgramRunning;
} }
bool RobotState::isProgramPaused() { bool RobotState::isProgramPaused()
return robot_mode_.isProgramPaused; {
return robot_mode_.isProgramPaused;
} }
unsigned char RobotState::getRobotMode() { unsigned char RobotState::getRobotMode()
return robot_mode_.robotMode; {
return robot_mode_.robotMode;
} }
bool RobotState::isReady() { bool RobotState::isReady()
if (robot_mode_.robotMode == robot_mode_running_) { {
return true; if (robot_mode_.robotMode == robot_mode_running_) {
} return true;
return false; }
return false;
} }
void RobotState::setDisconnected() { void RobotState::setDisconnected()
robot_mode_.isRobotConnected = false; {
robot_mode_.isRealRobotEnabled = false; robot_mode_.isRobotConnected = false;
robot_mode_.isPowerOnRobot = 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" #include "ur_modern_driver/robot_state_RT.h"
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { RobotStateRT::RobotStateRT(std::condition_variable& msg_cond)
version_ = 0.0; {
time_ = 0.0; version_ = 0.0;
q_target_.assign(6, 0.0); time_ = 0.0;
qd_target_.assign(6, 0.0); q_target_.assign(6, 0.0);
qdd_target_.assign(6, 0.0); qd_target_.assign(6, 0.0);
i_target_.assign(6, 0.0); qdd_target_.assign(6, 0.0);
m_target_.assign(6, 0.0); i_target_.assign(6, 0.0);
q_actual_.assign(6, 0.0); m_target_.assign(6, 0.0);
qd_actual_.assign(6, 0.0); q_actual_.assign(6, 0.0);
i_actual_.assign(6, 0.0); qd_actual_.assign(6, 0.0);
i_control_.assign(6, 0.0); i_actual_.assign(6, 0.0);
tool_vector_actual_.assign(6, 0.0); i_control_.assign(6, 0.0);
tcp_speed_actual_.assign(6, 0.0); tool_vector_actual_.assign(6, 0.0);
tcp_force_.assign(6, 0.0); tcp_speed_actual_.assign(6, 0.0);
tool_vector_target_.assign(6, 0.0); tcp_force_.assign(6, 0.0);
tcp_speed_target_.assign(6, 0.0); tool_vector_target_.assign(6, 0.0);
digital_input_bits_.assign(64, false); tcp_speed_target_.assign(6, 0.0);
motor_temperatures_.assign(6, 0.0); digital_input_bits_.assign(64, false);
controller_timer_ = 0.0; motor_temperatures_.assign(6, 0.0);
robot_mode_ = 0.0; controller_timer_ = 0.0;
joint_modes_.assign(6, 0.0); robot_mode_ = 0.0;
safety_mode_ = 0.0; joint_modes_.assign(6, 0.0);
tool_accelerometer_values_.assign(3, 0.0); safety_mode_ = 0.0;
speed_scaling_ = 0.0; tool_accelerometer_values_.assign(3, 0.0);
linear_momentum_norm_ = 0.0; speed_scaling_ = 0.0;
v_main_ = 0.0; linear_momentum_norm_ = 0.0;
v_robot_ = 0.0; v_main_ = 0.0;
i_robot_ = 0.0; v_robot_ = 0.0;
v_actual_.assign(6, 0.0); i_robot_ = 0.0;
data_published_ = false; v_actual_.assign(6, 0.0);
controller_updated_ = false; data_published_ = false;
pMsg_cond_ = &msg_cond; controller_updated_ = false;
pMsg_cond_ = &msg_cond;
} }
RobotStateRT::~RobotStateRT() { RobotStateRT::~RobotStateRT()
/* Make sure nobody is waiting after this thread is destroyed */ {
data_published_ = true; /* Make sure nobody is waiting after this thread is destroyed */
controller_updated_ = true; data_published_ = true;
pMsg_cond_->notify_all(); controller_updated_ = true;
pMsg_cond_->notify_all();
} }
void RobotStateRT::setDataPublished() { void RobotStateRT::setDataPublished()
data_published_ = false; {
data_published_ = false;
} }
bool RobotStateRT::getDataPublished() { bool RobotStateRT::getDataPublished()
return data_published_; {
return data_published_;
} }
void RobotStateRT::setControllerUpdated() { void RobotStateRT::setControllerUpdated()
controller_updated_ = false; {
controller_updated_ = false;
} }
bool RobotStateRT::getControllerUpdated() { bool RobotStateRT::getControllerUpdated()
return controller_updated_; {
return controller_updated_;
} }
double RobotStateRT::ntohd(uint64_t nf) { double RobotStateRT::ntohd(uint64_t nf)
double x; {
nf = be64toh(nf); double x;
memcpy(&x, &nf, sizeof(x)); nf = be64toh(nf);
return x; memcpy(&x, &nf, sizeof(x));
return x;
} }
std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index, std::vector<double> RobotStateRT::unpackVector(uint8_t* buf, int start_index,
int nr_of_vals) { int nr_of_vals)
uint64_t q; {
std::vector<double> ret; uint64_t q;
for (int i = 0; i < nr_of_vals; i++) { std::vector<double> ret;
memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q)); for (int i = 0; i < nr_of_vals; i++) {
ret.push_back(ntohd(q)); memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
} ret.push_back(ntohd(q));
return ret; }
return ret;
} }
std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) { std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data)
std::vector<bool> ret; {
for (int i = 0; i < 64; i++) { std::vector<bool> ret;
ret.push_back((data & (1 << i)) >> i); for (int i = 0; i < 64; i++) {
} ret.push_back((data & (1 << i)) >> i);
return ret; }
return ret;
} }
void RobotStateRT::setVersion(double ver) { void RobotStateRT::setVersion(double ver)
val_lock_.lock(); {
version_ = ver; val_lock_.lock();
val_lock_.unlock(); version_ = ver;
val_lock_.unlock();
} }
double RobotStateRT::getVersion() { double RobotStateRT::getVersion()
double ret; {
val_lock_.lock(); double ret;
ret = version_; val_lock_.lock();
val_lock_.unlock(); ret = version_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getTime() { double RobotStateRT::getTime()
double ret; {
val_lock_.lock(); double ret;
ret = time_; val_lock_.lock();
val_lock_.unlock(); ret = time_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getQTarget() { std::vector<double> RobotStateRT::getQTarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = q_target_; val_lock_.lock();
val_lock_.unlock(); ret = q_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getQdTarget() { std::vector<double> RobotStateRT::getQdTarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = qd_target_; val_lock_.lock();
val_lock_.unlock(); ret = qd_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getQddTarget() { std::vector<double> RobotStateRT::getQddTarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = qdd_target_; val_lock_.lock();
val_lock_.unlock(); ret = qdd_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getITarget() { std::vector<double> RobotStateRT::getITarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = i_target_; val_lock_.lock();
val_lock_.unlock(); ret = i_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getMTarget() { std::vector<double> RobotStateRT::getMTarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = m_target_; val_lock_.lock();
val_lock_.unlock(); ret = m_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getQActual() { std::vector<double> RobotStateRT::getQActual()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = q_actual_; val_lock_.lock();
val_lock_.unlock(); ret = q_actual_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getQdActual() { std::vector<double> RobotStateRT::getQdActual()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = qd_actual_; val_lock_.lock();
val_lock_.unlock(); ret = qd_actual_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getIActual() { std::vector<double> RobotStateRT::getIActual()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = i_actual_; val_lock_.lock();
val_lock_.unlock(); ret = i_actual_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getIControl() { std::vector<double> RobotStateRT::getIControl()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = i_control_; val_lock_.lock();
val_lock_.unlock(); ret = i_control_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getToolVectorActual() { std::vector<double> RobotStateRT::getToolVectorActual()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = tool_vector_actual_; val_lock_.lock();
val_lock_.unlock(); ret = tool_vector_actual_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getTcpSpeedActual() { std::vector<double> RobotStateRT::getTcpSpeedActual()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = tcp_speed_actual_; val_lock_.lock();
val_lock_.unlock(); ret = tcp_speed_actual_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getTcpForce() { std::vector<double> RobotStateRT::getTcpForce()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = tcp_force_; val_lock_.lock();
val_lock_.unlock(); ret = tcp_force_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getToolVectorTarget() { std::vector<double> RobotStateRT::getToolVectorTarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = tool_vector_target_; val_lock_.lock();
val_lock_.unlock(); ret = tool_vector_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getTcpSpeedTarget() { std::vector<double> RobotStateRT::getTcpSpeedTarget()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = tcp_speed_target_; val_lock_.lock();
val_lock_.unlock(); ret = tcp_speed_target_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<bool> RobotStateRT::getDigitalInputBits() { std::vector<bool> RobotStateRT::getDigitalInputBits()
std::vector<bool> ret; {
val_lock_.lock(); std::vector<bool> ret;
ret = digital_input_bits_; val_lock_.lock();
val_lock_.unlock(); ret = digital_input_bits_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getMotorTemperatures() { std::vector<double> RobotStateRT::getMotorTemperatures()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = motor_temperatures_; val_lock_.lock();
val_lock_.unlock(); ret = motor_temperatures_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getControllerTimer() { double RobotStateRT::getControllerTimer()
double ret; {
val_lock_.lock(); double ret;
ret = controller_timer_; val_lock_.lock();
val_lock_.unlock(); ret = controller_timer_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getRobotMode() { double RobotStateRT::getRobotMode()
double ret; {
val_lock_.lock(); double ret;
ret = robot_mode_; val_lock_.lock();
val_lock_.unlock(); ret = robot_mode_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getJointModes() { std::vector<double> RobotStateRT::getJointModes()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = joint_modes_; val_lock_.lock();
val_lock_.unlock(); ret = joint_modes_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getSafety_mode() { double RobotStateRT::getSafety_mode()
double ret; {
val_lock_.lock(); double ret;
ret = safety_mode_; val_lock_.lock();
val_lock_.unlock(); ret = safety_mode_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getToolAccelerometerValues() { std::vector<double> RobotStateRT::getToolAccelerometerValues()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = tool_accelerometer_values_; val_lock_.lock();
val_lock_.unlock(); ret = tool_accelerometer_values_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getSpeedScaling() { double RobotStateRT::getSpeedScaling()
double ret; {
val_lock_.lock(); double ret;
ret = speed_scaling_; val_lock_.lock();
val_lock_.unlock(); ret = speed_scaling_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getLinearMomentumNorm() { double RobotStateRT::getLinearMomentumNorm()
double ret; {
val_lock_.lock(); double ret;
ret = linear_momentum_norm_; val_lock_.lock();
val_lock_.unlock(); ret = linear_momentum_norm_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getVMain() { double RobotStateRT::getVMain()
double ret; {
val_lock_.lock(); double ret;
ret = v_main_; val_lock_.lock();
val_lock_.unlock(); ret = v_main_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getVRobot() { double RobotStateRT::getVRobot()
double ret; {
val_lock_.lock(); double ret;
ret = v_robot_; val_lock_.lock();
val_lock_.unlock(); ret = v_robot_;
return ret; val_lock_.unlock();
return ret;
} }
double RobotStateRT::getIRobot() { double RobotStateRT::getIRobot()
double ret; {
val_lock_.lock(); double ret;
ret = i_robot_; val_lock_.lock();
val_lock_.unlock(); ret = i_robot_;
return ret; val_lock_.unlock();
return ret;
} }
std::vector<double> RobotStateRT::getVActual() { std::vector<double> RobotStateRT::getVActual()
std::vector<double> ret; {
val_lock_.lock(); std::vector<double> ret;
ret = v_actual_; val_lock_.lock();
val_lock_.unlock(); ret = v_actual_;
return ret; val_lock_.unlock();
return ret;
} }
void RobotStateRT::unpack(uint8_t * buf) { void RobotStateRT::unpack(uint8_t* buf)
int64_t digital_input_bits; {
uint64_t unpack_to; int64_t digital_input_bits;
uint16_t offset = 0; uint64_t unpack_to;
val_lock_.lock(); uint16_t offset = 0;
int len; val_lock_.lock();
memcpy(&len, &buf[offset], sizeof(len)); int len;
memcpy(&len, &buf[offset], sizeof(len));
offset += sizeof(len); offset += sizeof(len);
len = ntohl(len); len = ntohl(len);
//Check the correct message length is received //Check the correct message length is received
bool len_good = true; bool len_good = true;
if (version_ >= 1.6 && version_ < 1.7) { //v1.6 if (version_ >= 1.6 && version_ < 1.7) { //v1.6
if (len != 756) if (len != 756)
len_good = false; len_good = false;
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7 } else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
if (len != 764) if (len != 764)
len_good = false; len_good = false;
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8 } else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
if (len != 812) if (len != 812)
len_good = false; len_good = false;
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1 } else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
if (len != 1044) if (len != 1044)
len_good = false; len_good = false;
} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2 } else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
if (len != 1060) if (len != 1060)
len_good = false; len_good = false;
} }
if (!len_good) { if (!len_good) {
printf("Wrong length of message on RT interface: %i\n", len); printf("Wrong length of message on RT interface: %i\n", len);
val_lock_.unlock(); val_lock_.unlock();
return; return;
} }
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = RobotStateRT::ntohd(unpack_to); time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double); offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 6); q_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
qd_target_ = unpackVector(buf, offset, 6); qd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
qdd_target_ = unpackVector(buf, offset, 6); qdd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
i_target_ = unpackVector(buf, offset, 6); i_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
m_target_ = unpackVector(buf, offset, 6); m_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
q_actual_ = unpackVector(buf, offset, 6); q_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
qd_actual_ = unpackVector(buf, offset, 6); qd_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
i_actual_ = unpackVector(buf, offset, 6); i_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
if (version_ <= 1.9) { if (version_ <= 1.9) {
if (version_ > 1.6) if (version_ > 1.6)
tool_accelerometer_values_ = unpackVector(buf, offset, 3); tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * (3 + 15); offset += sizeof(double) * (3 + 15);
tcp_force_ = unpackVector(buf, offset, 6); tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6); tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6); tcp_speed_actual_ = unpackVector(buf, offset, 6);
} else { } else {
i_control_ = unpackVector(buf, offset, 6); i_control_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6); tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6); tcp_speed_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tcp_force_ = unpackVector(buf, offset, 6); tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tool_vector_target_ = unpackVector(buf, offset, 6); tool_vector_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;
tcp_speed_target_ = unpackVector(buf, offset, 6); tcp_speed_target_ = unpackVector(buf, offset, 6);
} }
offset += sizeof(double) * 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(&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" #include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) { bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
sensor_msgs::JointState joint_msg; {
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t; joint_msg.header.stamp = t;
joint_msg.name = _joint_names; 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); 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); 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); joint_msg.effort.push_back(i);
} }
@@ -20,7 +21,8 @@ bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
return true; 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; geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t; wrench_msg.header.stamp = t;
@@ -36,7 +38,8 @@ bool RTPublisher::publish_wrench(RTShared &packet, ros::Time &t) {
return true; 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; geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t; tool_twist.header.stamp = t;
@@ -55,20 +58,25 @@ bool RTPublisher::publish_tool(RTShared &packet, ros::Time &t) {
return true; return true;
} }
bool RTPublisher::publish(RTShared &packet) { bool RTPublisher::publish(RTShared& packet)
{
ros::Time time = ros::Time::now(); ros::Time time = ros::Time::now();
return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time); 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); return publish(state);
} }
bool RTPublisher::consume(RTState_V1_8 &state) { bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state); return publish(state);
} }
bool RTPublisher::consume(RTState_V3_0__1 &state) { bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state); return publish(state);
} }
bool RTPublisher::consume(RTState_V3_2__3 &state) { bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state); return publish(state);
} }

93
src/ros_main.cpp
View File

@@ -1,18 +1,18 @@
#include <cstdlib>
#include <string>
#include <chrono> #include <chrono>
#include <thread> #include <cstdlib>
#include <ros/ros.h> #include <ros/ros.h>
#include <string>
#include <thread>
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.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/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 IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port"); 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 BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame");
struct ProgArgs { struct ProgArgs {
public: public:
std::string host; std::string host;
std::string prefix; std::string prefix;
std::string base_frame; std::string base_frame;
std::string tool_frame; std::string tool_frame;
int32_t reverse_port; int32_t reverse_port;
bool use_sim_time; bool use_sim_time;
}; };
bool parse_args(ProgArgs &args) { bool parse_args(ProgArgs& args)
if(!ros::param::get(IP_ADDR_ARG, args.host)) { {
LOG_ERROR("robot_ip_address parameter must be set!"); if (!ros::param::get(IP_ADDR_ARG, args.host)) {
return false; 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(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
return true; 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"); ros::init(argc, argv, "ur_driver");
ProgArgs args; ProgArgs args;
if(!parse_args(args)) { if (!parse_args(args)) {
return EXIT_FAILURE; 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); URStream s2(args.host, 30003);
URProducer<RTPacket> p2(s2, *parser); URProducer<RTPacket> p2(s2, *parser);
/* /*
p2.setup_producer(); p2.setup_producer();
while(true) { while(true) {
@@ -77,18 +77,17 @@ int main(int argc, char **argv) {
p2.teardown_producer(); 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()) { while (ros::ok()) {
std::this_thread::sleep_for(std::chrono::milliseconds(33)); 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/master_board.h"
#include "ur_modern_driver/ur/consumer.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_range0);
bp.parse(analog_input_range1); bp.parse(analog_input_range1);
bp.parse(analog_input0); bp.parse(analog_input0);
@@ -17,8 +18,9 @@ bool SharedMasterBoardData::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V1_X::parse_with(BinParser &bp) { bool MasterBoardData_V1_X::parse_with(BinParser& bp)
if(!bp.check_size<MasterBoardData_V1_X>()) {
if (!bp.check_size<MasterBoardData_V1_X>())
return false; return false;
bp.parse(digital_input_bits); 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(master_on_off_state);
bp.parse(euromap67_interface_installed); bp.parse(euromap67_interface_installed);
if(euromap67_interface_installed) { if (euromap67_interface_installed) {
if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE)) if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
return false; return false;
bp.parse(euromap_voltage); bp.parse(euromap_voltage);
@@ -41,8 +43,9 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) { bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
if(!bp.check_size<MasterBoardData_V3_0__1>()) {
if (!bp.check_size<MasterBoardData_V3_0__1>())
return false; return false;
bp.parse(digital_input_bits); 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(in_reduced_mode);
bp.parse(euromap67_interface_installed); bp.parse(euromap67_interface_installed);
if(euromap67_interface_installed) { if (euromap67_interface_installed) {
if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE)) if (!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
return false; return false;
bp.parse(euromap_voltage); bp.parse(euromap_voltage);
@@ -67,10 +70,9 @@ bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
{
bool MasterBoardData_V3_2::parse_with(BinParser &bp) { if (!bp.check_size<MasterBoardData_V3_2>())
if(!bp.check_size<MasterBoardData_V3_2>())
return false; return false;
MasterBoardData_V3_0__1::parse_with(bp); MasterBoardData_V3_0__1::parse_with(bp);
@@ -81,16 +83,15 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
return true; return true;
} }
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) {
return consumer.consume(*this); 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); return consumer.consume(*this);
} }
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) { bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this); 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/messages.h"
#include "ur_modern_driver/ur/consumer.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(project_name);
bp.parse(major_version); bp.parse(major_version);
@@ -14,7 +14,7 @@ bool VersionMessage::parse_with(BinParser &bp) {
return true; //not really possible to check dynamic size packets 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); 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/robot_mode.h"
#include "ur_modern_driver/ur/consumer.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(timestamp);
bp.parse(physical_robot_connected); bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled); bp.parse(real_robot_enabled);
@@ -11,9 +11,9 @@ bool SharedRobotModeData::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V1_X::parse_with(BinParser& bp)
bool RobotModeData_V1_X::parse_with(BinParser &bp) { {
if(!bp.check_size<RobotModeData_V1_X>()) if (!bp.check_size<RobotModeData_V1_X>())
return false; return false;
SharedRobotModeData::parse_with(bp); SharedRobotModeData::parse_with(bp);
@@ -27,9 +27,9 @@ bool RobotModeData_V1_X::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
bool RobotModeData_V3_0__1::parse_with(BinParser &bp) { {
if(!bp.check_size<RobotModeData_V3_0__1>()) if (!bp.check_size<RobotModeData_V3_0__1>())
return false; return false;
SharedRobotModeData::parse_with(bp); SharedRobotModeData::parse_with(bp);
@@ -45,8 +45,9 @@ bool RobotModeData_V3_0__1::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V3_2::parse_with(BinParser &bp) { bool RobotModeData_V3_2::parse_with(BinParser& bp)
if(!bp.check_size<RobotModeData_V3_2>()) {
if (!bp.check_size<RobotModeData_V3_2>())
return false; return false;
RobotModeData_V3_0__1::parse_with(bp); RobotModeData_V3_0__1::parse_with(bp);
@@ -56,15 +57,15 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) {
return true; return true;
} }
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
{
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) {
return consumer.consume(*this); 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); return consumer.consume(*this);
} }
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) { bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
{
return consumer.consume(*this); 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/rt_state.h"
#include "ur_modern_driver/ur/consumer.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(time);
bp.parse(q_target); bp.parse(q_target);
bp.parse(qd_target); bp.parse(qd_target);
@@ -15,7 +15,8 @@ bool RTShared::parse_shared1(BinParser &bp) {
return true; return true;
} }
bool RTShared::parse_shared2(BinParser &bp) { bool RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_input); bp.parse(digital_input);
bp.parse(motor_temperatures); bp.parse(motor_temperatures);
bp.parse(controller_time); bp.parse(controller_time);
@@ -23,9 +24,9 @@ bool RTShared::parse_shared2(BinParser &bp) {
return true; return true;
} }
bool RTState_V1_6__7::parse_with(BinParser& bp)
bool RTState_V1_6__7::parse_with(BinParser &bp) { {
if(!bp.check_size<RTState_V1_6__7>()) if (!bp.check_size<RTState_V1_6__7>())
return false; return false;
parse_shared1(bp); parse_shared1(bp);
@@ -40,8 +41,9 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V1_8::parse_with(BinParser &bp) { bool RTState_V1_8::parse_with(BinParser& bp)
if(!bp.check_size<RTState_V1_8>()) {
if (!bp.check_size<RTState_V1_8>())
return false; return false;
RTState_V1_6__7::parse_with(bp); RTState_V1_6__7::parse_with(bp);
@@ -51,8 +53,9 @@ bool RTState_V1_8::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V3_0__1::parse_with(BinParser &bp) { bool RTState_V3_0__1::parse_with(BinParser& bp)
if(!bp.check_size<RTState_V3_0__1>()) {
if (!bp.check_size<RTState_V3_0__1>())
return false; return false;
parse_shared1(bp); parse_shared1(bp);
@@ -83,8 +86,9 @@ bool RTState_V3_0__1::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V3_2__3::parse_with(BinParser &bp) { bool RTState_V3_2__3::parse_with(BinParser& bp)
if(!bp.check_size<RTState_V3_2__3>()) {
if (!bp.check_size<RTState_V3_2__3>())
return false; return false;
RTState_V3_0__1::parse_with(bp); RTState_V3_0__1::parse_with(bp);
@@ -95,15 +99,19 @@ bool RTState_V3_2__3::parse_with(BinParser &bp) {
return true; return true;
} }
bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) { bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this); return consumer.consume(*this);
} }
bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) { bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
{
return consumer.consume(*this); 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); 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); 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/ur/state.h"
#include "ur_modern_driver/log.h"
//StatePacket::~StatePacket() { } //StatePacket::~StatePacket() { }

58
src/ur/stream.cpp
View File

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

300
src/ur_communication.cpp
View File

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

638
src/ur_driver.cpp
View File

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

389
src/ur_hardware_interface.cpp
View File

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

311
src/ur_realtime_communication.cpp
View File

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

1356
src/ur_ros_wrapper.cpp
View File

File diff suppressed because it is too large Load Diff