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

Adopted roscpp code style and naming convention

Browse Source
This commit is contained in:
Simon Rasmussen
2017-03-01 13:59:48 +01:00
parent e4bc40fc09
commit 474f469e97
44 changed files with 5097 additions and 4891 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/do_output.cpp
View File

@@ -21,42 +21,42 @@
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)
{
#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)
{
#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)
{
#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)
{
#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
}

577
src/robot_state.cpp
View File

@@ -20,389 +20,380 @@
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;
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 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;
/* 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;
}
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)
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) {
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();
version_msg_.timestamp = timestamp;
version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len);
RobotState::unpackRobotMode(buf, offset + 5);
val_lock_.unlock();
break;
default:
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)
void RobotState::unpackRobotMessageVersion(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;
}
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(&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_.speedScaling = RobotState::ntohd(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)
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);
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_.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_.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);
}
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_.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);
}
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 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;
new_data_available_ = false;
}
bool RobotState::getNewDataAvailable()
{
return new_data_available_;
return new_data_available_;
}
int RobotState::getDigitalInputBits()
{
return mb_data_.digitalInputBits;
return mb_data_.digitalInputBits;
}
int RobotState::getDigitalOutputBits()
{
return mb_data_.digitalOutputBits;
return mb_data_.digitalOutputBits;
}
double RobotState::getAnalogInput0()
{
return mb_data_.analogInput0;
return mb_data_.analogInput0;
}
double RobotState::getAnalogInput1()
{
return mb_data_.analogInput1;
return mb_data_.analogInput1;
}
double RobotState::getAnalogOutput0()
{
return mb_data_.analogOutput0;
return mb_data_.analogOutput0;
}
double RobotState::getAnalogOutput1()
{
return mb_data_.analogOutput1;
return mb_data_.analogOutput1;
}
bool RobotState::isRobotConnected()
{
return robot_mode_.isRobotConnected;
return robot_mode_.isRobotConnected;
}
bool RobotState::isRealRobotEnabled()
{
return robot_mode_.isRealRobotEnabled;
return robot_mode_.isRealRobotEnabled;
}
bool RobotState::isPowerOnRobot()
{
return robot_mode_.isPowerOnRobot;
return robot_mode_.isPowerOnRobot;
}
bool RobotState::isEmergencyStopped()
{
return robot_mode_.isEmergencyStopped;
return robot_mode_.isEmergencyStopped;
}
bool RobotState::isProtectiveStopped()
{
return robot_mode_.isProtectiveStopped;
return robot_mode_.isProtectiveStopped;
}
bool RobotState::isProgramRunning()
{
return robot_mode_.isProgramRunning;
return robot_mode_.isProgramRunning;
}
bool RobotState::isProgramPaused()
{
return robot_mode_.isProgramPaused;
return robot_mode_.isProgramPaused;
}
unsigned char RobotState::getRobotMode()
{
return robot_mode_.robotMode;
return robot_mode_.robotMode;
}
bool RobotState::isReady()
{
if (robot_mode_.robotMode == robot_mode_running_) {
return true;
}
return false;
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;
robot_mode_.isRobotConnected = false;
robot_mode_.isRealRobotEnabled = false;
robot_mode_.isPowerOnRobot = false;
}

665
src/robot_state_RT.cpp
View File

@@ -20,456 +20,473 @@
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;
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();
/* 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;
data_published_ = false;
}
bool RobotStateRT::getDataPublished()
{
return data_published_;
return data_published_;
}
void RobotStateRT::setControllerUpdated()
{
controller_updated_ = false;
controller_updated_ = false;
}
bool RobotStateRT::getControllerUpdated()
{
return controller_updated_;
return controller_updated_;
}
double RobotStateRT::ntohd(uint64_t nf)
{
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
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)
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;
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> 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();
val_lock_.lock();
version_ = ver;
val_lock_.unlock();
}
double RobotStateRT::getVersion()
{
double ret;
val_lock_.lock();
ret = version_;
val_lock_.unlock();
return ret;
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 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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> 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 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 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> 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 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> 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 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 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 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 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 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> 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));
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;
}
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);
}
if (!len_good)
{
printf("Wrong length of message on RT interface: %i\n", len);
val_lock_.unlock();
controller_updated_ = true;
data_published_ = true;
pMsg_cond_->notify_all();
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();
}

151
src/ros/rt_publisher.cpp
View File

@@ -1,119 +1,122 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared& packet, Time& t)
bool RTPublisher::publishJoints(RTShared& packet, Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = _joint_names;
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = joint_names_;
for (auto const& q : packet.q_actual) {
joint_msg.position.push_back(q);
}
for (auto const& qd : packet.qd_actual) {
joint_msg.velocity.push_back(qd);
}
for (auto const& i : packet.i_actual) {
joint_msg.effort.push_back(i);
}
for (auto const& q : packet.q_actual)
{
joint_msg.position.push_back(q);
}
for (auto const& qd : packet.qd_actual)
{
joint_msg.velocity.push_back(qd);
}
for (auto const& i : packet.i_actual)
{
joint_msg.effort.push_back(i);
}
_joint_pub.publish(joint_msg);
joint_pub_.publish(joint_msg);
return true;
return true;
}
bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
bool RTPublisher::publishWrench(RTShared& packet, Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
wrench_msg.wrench.force.x = packet.tcp_force[0];
wrench_msg.wrench.force.y = packet.tcp_force[1];
wrench_msg.wrench.force.z = packet.tcp_force[2];
wrench_msg.wrench.torque.x = packet.tcp_force[3];
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
wrench_msg.wrench.force.x = packet.tcp_force[0];
wrench_msg.wrench.force.y = packet.tcp_force[1];
wrench_msg.wrench.force.z = packet.tcp_force[2];
wrench_msg.wrench.torque.x = packet.tcp_force[3];
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
_wrench_pub.publish(wrench_msg);
return true;
wrench_pub_.publish(wrench_msg);
return true;
}
bool RTPublisher::publish_tool(RTShared& packet, Time& t)
bool RTPublisher::publishTool(RTShared& packet, Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
tool_twist.header.frame_id = _base_frame;
tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
tool_twist.header.frame_id = base_frame_;
tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
_tool_vel_pub.publish(tool_twist);
return true;
tool_vel_pub_.publish(tool_twist);
return true;
}
bool RTPublisher::publish_transform(RTShared& packet, Time& t)
bool RTPublisher::publishTransform(RTShared& packet, Time& t)
{
auto tv = packet.tool_vector_actual;
auto tv = packet.tool_vector_actual;
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
// Create quaternion
Quaternion quat;
//Create quaternion
Quaternion quat;
double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
if (angle < 1e-16)
{
quat.setValue(0, 0, 0, 1);
}
else
{
quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
}
double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
if (angle < 1e-16) {
quat.setValue(0, 0, 0, 1);
} else {
quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
}
transform.setRotation(quat);
transform.setRotation(quat);
transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
_transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
return true;
return true;
}
bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
{
size_t len = _joint_names.size();
for (size_t i = 0; i < len; i++) {
sensor_msgs::Temperature msg;
msg.header.stamp = t;
msg.header.frame_id = _joint_names[i];
msg.temperature = packet.motor_temperatures[i];
size_t len = joint_names_.size();
for (size_t i = 0; i < len; i++)
{
sensor_msgs::Temperature msg;
msg.header.stamp = t;
msg.header.frame_id = joint_names_[i];
msg.temperature = packet.motor_temperatures[i];
_joint_temperature_pub.publish(msg);
}
return true;
joint_temperature_pub_.publish(msg);
}
return true;
}
bool RTPublisher::publish(RTShared& packet)
{
Time time = Time::now();
return publish_joints(packet, time)
&& publish_wrench(packet, time)
&& publish_tool(packet, time)
&& publish_transform(packet, time)
&& publish_temperature(packet, time);
Time time = Time::now();
return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
publishTransform(packet, time) && publishTemperature(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7& state)
{
return publish(state);
return publish(state);
}
bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state);
return publish(state);
}
bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state);
return publish(state);
}
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
return publish(state);
}

74
src/ros_main.cpp
View File

@@ -1,6 +1,6 @@
#include <ros/ros.h>
#include <chrono>
#include <cstdlib>
#include <ros/ros.h>
#include <string>
#include <thread>
@@ -21,55 +21,59 @@ 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 {
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;
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)
{
ros::init(argc, argv, "ur_driver");
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);
auto parser = factory.get_rt_parser();
URFactory factory(args.host);
auto parser = factory.getRTParser();
URStream s2(args.host, 30003);
URProducer<RTPacket> p2(s2, *parser);
RTPublisher pub(args.prefix, args.base_frame, args.tool_frame);
URStream s2(args.host, 30003);
URProducer<RTPacket> p2(s2, *parser);
RTPublisher pub(args.prefix, args.base_frame, args.tool_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;
}

122
src/ur/master_board.cpp
View File

@@ -1,97 +1,99 @@
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedMasterBoardData::parse_with(BinParser& bp)
bool SharedMasterBoardData::parseWith(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
bp.parse(analog_input1);
bp.parse(analog_output_domain0);
bp.parse(analog_output_domain1);
bp.parse(analog_output0);
bp.parse(analog_output1);
bp.parse(master_board_temperature);
bp.parse(robot_voltage_48V);
bp.parse(robot_current);
bp.parse(master_IO_current);
return true;
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
bp.parse(analog_input1);
bp.parse(analog_output_domain0);
bp.parse(analog_output_domain1);
bp.parse(analog_output0);
bp.parse(analog_output1);
bp.parse(master_board_temperature);
bp.parse(robot_voltage_48V);
bp.parse(robot_current);
bp.parse(master_IO_current);
return true;
}
bool MasterBoardData_V1_X::parse_with(BinParser& bp)
bool MasterBoardData_V1_X::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V1_X>())
return false;
if (!bp.checkSize<MasterBoardData_V1_X>())
return false;
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
SharedMasterBoardData::parse_with(bp);
SharedMasterBoardData::parseWith(bp);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed) {
if (!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
return false;
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
return true;
return true;
}
bool MasterBoardData_V3_0__1::parse_with(BinParser& bp)
bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_0__1>())
return false;
if (!bp.checkSize<MasterBoardData_V3_0__1>())
return false;
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
bp.parse(digital_input_bits);
bp.parse(digital_output_bits);
SharedMasterBoardData::parse_with(bp);
SharedMasterBoardData::parseWith(bp);
bp.parse(safety_mode);
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
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))
return false;
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.consume(sizeof(uint32_t));
bp.consume(sizeof(uint32_t));
return true;
return true;
}
bool MasterBoardData_V3_2::parse_with(BinParser& bp)
bool MasterBoardData_V3_2::parseWith(BinParser& bp)
{
if (!bp.check_size<MasterBoardData_V3_2>())
return false;
if (!bp.checkSize<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parse_with(bp);
MasterBoardData_V3_0__1::parseWith(bp);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
return true;
return true;
}
bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer& consumer)
bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

21
src/ur/messages.cpp
View File

@@ -1,20 +1,19 @@
#include "ur_modern_driver/ur/messages.h"
#include "ur_modern_driver/ur/consumer.h"
bool VersionMessage::parse_with(BinParser& bp)
bool VersionMessage::parseWith(BinParser& bp)
{
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
bp.parse(svn_version);
bp.consume(sizeof(uint32_t)); // undocumented field??
bp.parse_remainder(build_date);
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
bp.parse(svn_version);
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::consumeWith(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

82
src/ur/robot_mode.cpp
View File

@@ -1,71 +1,71 @@
#include "ur_modern_driver/ur/robot_mode.h"
#include "ur_modern_driver/ur/consumer.h"
bool SharedRobotModeData::parse_with(BinParser& bp)
bool SharedRobotModeData::parseWith(BinParser& bp)
{
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
bp.parse(robot_power_on);
bp.parse(emergency_stopped);
return true;
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
bp.parse(robot_power_on);
bp.parse(emergency_stopped);
return true;
}
bool RobotModeData_V1_X::parse_with(BinParser& bp)
bool RobotModeData_V1_X::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V1_X>())
return false;
if (!bp.checkSize<RobotModeData_V1_X>())
return false;
SharedRobotModeData::parse_with(bp);
SharedRobotModeData::parseWith(bp);
bp.parse(security_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(speed_fraction);
bp.parse(security_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(speed_fraction);
return true;
return true;
}
bool RobotModeData_V3_0__1::parse_with(BinParser& bp)
bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_0__1>())
return false;
if (!bp.checkSize<RobotModeData_V3_0__1>())
return false;
SharedRobotModeData::parse_with(bp);
SharedRobotModeData::parseWith(bp);
bp.parse(protective_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(control_mode);
bp.parse(target_speed_fraction);
bp.parse(speed_scaling);
bp.parse(protective_stopped);
bp.parse(program_running);
bp.parse(program_paused);
bp.parse(robot_mode);
bp.parse(control_mode);
bp.parse(target_speed_fraction);
bp.parse(speed_scaling);
return true;
return true;
}
bool RobotModeData_V3_2::parse_with(BinParser& bp)
bool RobotModeData_V3_2::parseWith(BinParser& bp)
{
if (!bp.check_size<RobotModeData_V3_2>())
return false;
if (!bp.checkSize<RobotModeData_V3_2>())
return false;
RobotModeData_V3_0__1::parse_with(bp);
RobotModeData_V3_0__1::parseWith(bp);
bp.parse(target_speed_fraction_limit);
bp.parse(target_speed_fraction_limit);
return true;
return true;
}
bool RobotModeData_V1_X::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RobotModeData_V3_2::consume_with(URStatePacketConsumer& consumer)
bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

142
src/ur/rt_state.cpp
View File

@@ -3,115 +3,115 @@
void RTShared::parse_shared1(BinParser& bp)
{
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
bp.parse(qdd_target);
bp.parse(i_target);
bp.parse(m_target);
bp.parse(q_actual);
bp.parse(qd_actual);
bp.parse(i_actual);
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
bp.parse(qdd_target);
bp.parse(i_target);
bp.parse(m_target);
bp.parse(q_actual);
bp.parse(qd_actual);
bp.parse(i_actual);
}
void RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_inputs);
bp.parse(motor_temperatures);
bp.parse(controller_time);
bp.consume(sizeof(double)); //Unused "Test value" field
bp.parse(robot_mode);
bp.parse(digital_inputs);
bp.parse(motor_temperatures);
bp.parse(controller_time);
bp.consume(sizeof(double)); // Unused "Test value" field
bp.parse(robot_mode);
}
bool RTState_V1_6__7::parse_with(BinParser& bp)
bool RTState_V1_6__7::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V1_6__7>())
return false;
if (!bp.checkSize<RTState_V1_6__7>())
return false;
parse_shared1(bp);
parse_shared1(bp);
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double)*15);
bp.parse(tcp_force);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double) * 15);
bp.parse(tcp_force);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
parse_shared2(bp);
parse_shared2(bp);
return true;
return true;
}
bool RTState_V1_8::parse_with(BinParser& bp)
bool RTState_V1_8::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V1_8>())
return false;
if (!bp.checkSize<RTState_V1_8>())
return false;
RTState_V1_6__7::parse_with(bp);
RTState_V1_6__7::parseWith(bp);
bp.parse(joint_modes);
bp.parse(joint_modes);
return true;
return true;
}
bool RTState_V3_0__1::parse_with(BinParser& bp)
bool RTState_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V3_0__1>())
return false;
if (!bp.checkSize<RTState_V3_0__1>())
return false;
parse_shared1(bp);
parse_shared1(bp);
bp.parse(i_control);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tcp_force);
bp.parse(tool_vector_target);
bp.parse(tcp_speed_target);
bp.parse(i_control);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tcp_force);
bp.parse(tool_vector_target);
bp.parse(tcp_speed_target);
parse_shared2(bp);
parse_shared2(bp);
bp.parse(joint_modes);
bp.parse(safety_mode);
bp.consume(sizeof(double[6])); //skip undocumented
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double[6])); //skip undocumented
bp.parse(speed_scaling);
bp.parse(linear_momentum_norm);
bp.consume(sizeof(double)); //skip undocumented
bp.consume(sizeof(double)); //skip undocumented
bp.parse(v_main);
bp.parse(v_robot);
bp.parse(i_robot);
bp.parse(v_actual);
bp.parse(joint_modes);
bp.parse(safety_mode);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(speed_scaling);
bp.parse(linear_momentum_norm);
bp.consume(sizeof(double)); // skip undocumented
bp.consume(sizeof(double)); // skip undocumented
bp.parse(v_main);
bp.parse(v_robot);
bp.parse(i_robot);
bp.parse(v_actual);
return true;
return true;
}
bool RTState_V3_2__3::parse_with(BinParser& bp)
bool RTState_V3_2__3::parseWith(BinParser& bp)
{
if (!bp.check_size<RTState_V3_2__3>())
return false;
if (!bp.checkSize<RTState_V3_2__3>())
return false;
RTState_V3_0__1::parse_with(bp);
RTState_V3_0__1::parseWith(bp);
bp.parse(digital_outputs);
bp.parse(program_state);
bp.parse(digital_outputs);
bp.parse(program_state);
return true;
return true;
}
bool RTState_V1_6__7::consume_with(URRTPacketConsumer& consumer)
bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RTState_V1_8::consume_with(URRTPacketConsumer& consumer)
bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RTState_V3_0__1::consume_with(URRTPacketConsumer& consumer)
bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}
bool RTState_V3_2__3::consume_with(URRTPacketConsumer& consumer)
bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
return consumer.consume(*this);
}

28
src/ur/state.cpp
View File

@@ -1,25 +1,25 @@
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/log.h"
//StatePacket::~StatePacket() { }
// StatePacket::~StatePacket() { }
/*
bool RobotState::parse_with(BinParser &bp) {
bool RobotState::parseWith(BinParser &bp) {
//continue as long as there are bytes to read
while(!bp.empty()) {
if(!bp.check_size(sizeof(uint32_t))){
if(!bp.checkSize(sizeof(uint32_t))){
LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
return false;
return false;
}
uint32_t sub_size = bp.peek<uint32_t>();
if(!bp.check_size(static_cast<size_t>(sub_size))) {
if(!bp.checkSize(static_cast<size_t>(sub_size))) {
LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
return false;
}
//deconstruction of a sub parser will increment the position of the parent parser
//deconstruction of a sub parser will increment the position of the parent parser
BinParser sub_parser(bp, sub_size);
sub_parser.consume(sizeof(sub_size));
@@ -43,19 +43,19 @@ bool parse_base(BinParser &bp, T &pkg) {
case package_type::ROBOT_MODE_DATA:
LOG_DEBUG("Parsing robot_mode");
bp.consume(sizeof(package_type));
pkg.robot_mode.parse_with(bp);
pkg.robot_mode.parseWith(bp);
break;
case package_type::MASTERBOARD_DATA:
LOG_DEBUG("Parsing master_board");
bp.consume(sizeof(package_type));
pkg.master_board.parse_with(bp);
pkg.master_board.parseWith(bp);
break;
case package_type::TOOL_DATA:
case package_type::CARTESIAN_INFO:
case package_type::JOINT_DATA:
LOG_DEBUG("Skipping tool, cartesian or joint data");
//for unhandled packages we consume the rest of the
LOG_DEBUG("Skipping tool, cartesian or joint data");
//for unhandled packages we consume the rest of the
//package buffer
bp.consume();
break;
@@ -79,8 +79,8 @@ bool parse_advanced(BinParser &bp, T &pkg) {
case package_type::ADDITIONAL_INFO:
case package_type::CALIBRATION_DATA:
case package_type::FORCE_MODE_DATA:
LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");
//for unhandled packages we consume the rest of the
LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");
//for unhandled packages we consume the rest of the
//package buffer
bp.consume();
break;

175
src/ur/stream.cpp
View File

@@ -1,126 +1,133 @@
#include <cstring>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect()
{
if (_initialized)
return false;
if (initialized_)
return false;
LOG_INFO("Connecting to UR @ %s:%d", _host.c_str(), _port);
LOG_INFO("Connecting to UR @ %s:%d", host_.c_str(), port_);
//gethostbyname() is deprecated so use getadderinfo() as described in:
//http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
// gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
std::string service = std::to_string(_port);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
std::string service = std::to_string(port_);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
hints.ai_family = AF_UNSPEC;
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");
return false;
}
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) {
_socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
// loop through the list of addresses untill we find one that's connectable
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?
continue;
if (socket_fd_ == -1) // socket error?
continue;
if (::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) {
if (_stopping)
break;
else
continue; //try next addrinfo if connect fails
}
//disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
_initialized = true;
LOG_INFO("Connection successfully established");
if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
{
if (stopping_)
break;
else
continue; // try next addrinfo if connect fails
}
freeaddrinfo(result);
if (!_initialized)
LOG_ERROR("Connection failed");
// disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
initialized_ = true;
LOG_INFO("Connection successfully established");
break;
}
return _initialized;
freeaddrinfo(result);
if (!initialized_)
LOG_ERROR("Connection failed");
return initialized_;
}
void URStream::disconnect()
{
if (!_initialized || _stopping)
return;
if (!initialized_ || stopping_)
return;
LOG_INFO("Disconnecting from %s:%d", _host.c_str(), _port);
LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
_stopping = true;
close(_socket_fd);
_initialized = false;
stopping_ = true;
close(socket_fd_);
initialized_ = false;
}
ssize_t URStream::send(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
return -1;
if (_stopping)
return 0;
if (!initialized_)
return -1;
if (stopping_)
return 0;
size_t total = 0;
size_t remaining = buf_len;
size_t total = 0;
size_t remaining = 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)
return _stopping ? 0 : sent;
total += sent;
remaining -= sent;
}
// TODO: handle reconnect?
// handle partial sends
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;
}
return total;
return total;
}
ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!_initialized)
if (!initialized_)
return -1;
if (stopping_)
return 0;
size_t remainder = sizeof(int32_t);
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)
{
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if (remainder >= (buf_len - sizeof(int32_t)))
{
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return -1;
if (_stopping)
return 0;
}
initial = false;
}
size_t remainder = sizeof(int32_t);
uint8_t* buf_pos = buf;
bool initial = true;
buf_pos += read;
remainder -= read;
} while (remainder > 0);
do {
ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
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))) {
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return -1;
}
initial = false;
}
buf_pos += read;
remainder -= read;
} while (remainder > 0);
return buf_pos - buf;
return buf_pos - buf;
}

291
src/ur_communication.cpp
View File

@@ -18,168 +18,167 @@
#include "ur_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond,
std::string host)
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;
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_);
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();
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;
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");
}
}
}
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;
//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_);
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_);
}

602
src/ur_driver.cpp
View File

@@ -18,397 +18,403 @@
#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)
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;
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)
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;
/*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)
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::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j;
if (!UrDriver::uploadProg()) {
return false;
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;
}
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);
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);
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");
executing_traj_ = false;
rt_interface_->addCommandToQueue("stopj(10)\n");
}
bool UrDriver::uploadProg()
{
std::string cmd_str;
char buf[128];
cmd_str = "def driverProg():\n";
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;
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);
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;
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_);
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)
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);
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
}
std::vector<std::string> UrDriver::getJointNames()
{
return joint_names_;
return joint_names_;
}
void UrDriver::setJointNames(std::vector<std::string> jn)
{
joint_names_ = 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);
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);
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");
} 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) {
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);
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)
{
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);
}
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);
}
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;
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;
}
if (m > 0)
{
minimum_payload_ = m;
}
else
{
minimum_payload_ = 0;
}
}
void UrDriver::setMaxPayload(double m)
{
maximum_payload_ = m;
maximum_payload_ = m;
}
void UrDriver::setServojTime(double t)
{
if (t > 0.008) {
servoj_time_ = t;
} else {
servoj_time_ = 0.008;
}
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;
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;
if (g > 100)
{
if (g < 2000)
{
servoj_gain_ = g;
}
else
{
servoj_gain_ = 2000;
}
}
else
{
servoj_gain_ = 100;
}
}

367
src/ur_hardware_interface.cpp
View File

@@ -57,228 +57,227 @@
#include <ur_modern_driver/ur_hardware_interface.h>
namespace ros_control_ur {
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot)
: nh_(nh)
, robot_(robot)
namespace ros_control_ur
{
// 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());
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];
}
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;
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_);
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
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;
}
}
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)
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 = 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");
}
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");
}
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
} // namespace

305
src/ur_realtime_communication.cpp
View File

@@ -18,183 +18,194 @@
#include "ur_modern_driver/ur_realtime_communication.h"
UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max)
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;
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;
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();
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");
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)
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;
}
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;
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");
}
}
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_);
}
}
setSpeed(0., 0., 0., 0., 0., 0.);
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;
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;
safety_count_max_ = inp;
}
std::string UrRealtimeCommunication::getLocalIp()
{
return local_ip_;
return local_ip_;
}

1504
src/ur_ros_wrapper.cpp
View File

File diff suppressed because it is too large Load Diff