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

39
src/do_output.cpp
View File

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

671
src/robot_state.cpp
View File

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

766
src/robot_state_RT.cpp
View File

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

32
src/ros/rt_publisher.cpp
View File

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

93
src/ros_main.cpp
View File

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

47
src/ur/master_board.cpp
View File

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

10
src/ur/messages.cpp
View File

@@ -1,9 +1,9 @@
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.h"
bool VersionMessage::parse_with(BinParser& bp)
{
bool VersionMessage::parse_with(BinParser &bp) {
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
@@ -11,10 +11,10 @@ bool VersionMessage::parse_with(BinParser &bp) {
bp.consume(sizeof(uint32_t)); //undocumented field??
bp.parse_remainder(build_date);
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);
}

35
src/ur/robot_mode.cpp
View File

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

44
src/ur/rt_state.cpp
View File

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

4
src/ur/state.cpp
View File

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

68
src/ur/stream.cpp
View File

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

300
src/ur_communication.cpp
View File

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

638
src/ur_driver.cpp
View File

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

389
src/ur_hardware_interface.cpp
View File

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

313
src/ur_realtime_communication.cpp
View File

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

1356
src/ur_ros_wrapper.cpp
View File

File diff suppressed because it is too large Load Diff