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

Tested and bugfixed masterboard data parsing

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-16 13:38:21 +02:00
parent 699bc08ac6
commit fb95d2188d
7 changed files with 84 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
include/ur_modern_driver/robot_state.h
View File

@@ -14,6 +14,7 @@
#include <string.h> #include <string.h>
#include <mutex> #include <mutex>
#include <condition_variable> #include <condition_variable>
#include <netinet/in.h>
namespace message_types { namespace message_types {
enum message_type { enum message_type {
@@ -58,11 +59,11 @@ struct version_message {
int8_t source; int8_t source;
int8_t robot_message_type; int8_t robot_message_type;
int8_t project_name_size; int8_t project_name_size;
char* project_name; char project_name[15];
uint8_t major_version; uint8_t major_version;
uint8_t minor_version; uint8_t minor_version;
int svn_revision; int svn_revision;
char* build_date; char build_date[25];
}; };
struct masterboard_data { struct masterboard_data {
@@ -95,7 +96,7 @@ private:
version_message version_msg_; version_message version_msg_;
masterboard_data mb_data_; masterboard_data mb_data_;
std::mutex val_lock_; // Locks the variables while unpack parses data; std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
std::condition_variable* pMsg_cond_; //Signals that new vars are available std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool new_data_available_; //to avoid spurious wakes bool new_data_available_; //to avoid spurious wakes

2
include/ur_modern_driver/ur_communication.h
View File

@@ -32,7 +32,7 @@
class UrCommunication { class UrCommunication {
private: private:
int sockfd_; int pri_sockfd_, sec_sockfd_;
struct sockaddr_in pri_serv_addr_, sec_serv_addr_; struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
struct hostent *server_; struct hostent *server_;
bool keepalive_; bool keepalive_;

74
src/robot_state.cpp
View File

@@ -13,19 +13,24 @@ RobotState::RobotState(std::condition_variable& msg_cond) {
new_data_available_ = false; new_data_available_ = false;
pMsg_cond_ = &msg_cond; pMsg_cond_ = &msg_cond;
} }
double RobotState::ntohd(uint64_t nf) {
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
}
int RobotState::unpack(uint8_t* buf, unsigned int buf_length) { int RobotState::unpack(uint8_t* buf, unsigned int buf_length) {
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */ /* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0; unsigned int offset = 0;
while (buf_length > offset) { while (buf_length > offset) {
uint32_t len; int len;
unsigned char message_type; unsigned char message_type;
memcpy(&len, &buf[offset], sizeof(len)); memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len);
if (len + offset > buf_length) { if (len + offset > buf_length) {
return (len + offset - buf_length); return (len + offset - buf_length);
} }
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type)); memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
switch (message_type) { switch (message_type) {
case messageType::ROBOT_MESSAGE: case messageType::ROBOT_MESSAGE:
RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType RobotState::unpackRobotMessage(buf, offset, len); //'len' is inclusive the 5 bytes from messageSize and messageType
@@ -58,7 +63,7 @@ void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
switch (robot_message_type) { switch (robot_message_type) {
case robotMessageType::ROBOT_MESSAGE_VERSION: case robotMessageType::ROBOT_MESSAGE_VERSION:
val_lock_.lock(); val_lock_.lock();
version_msg_.timestamp = timestamp; version_msg_.timestamp = RobotState::ntohd(timestamp);
version_msg_.source = source; version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type; version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len); RobotState::unpackRobotMessageVersion(buf, offset, len);
@@ -74,16 +79,15 @@ void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
uint32_t len) { uint32_t len) {
offset += 5; offset += 5;
while (offset < len) { while (offset < len) {
uint32_t length; int32_t length;
int8_t package_type; uint8_t package_type;
memcpy(&length, &buf[offset], sizeof(length)); memcpy(&length, &buf[offset], sizeof(length));
offset += sizeof(length); length = ntohl(length);
memcpy(&package_type, &buf[offset], sizeof(package_type)); memcpy(&package_type, &buf[offset+sizeof(length)], sizeof(package_type));
offset += sizeof(package_type);
switch (package_type) { switch (package_type) {
case packageType::MASTERBOARD_DATA: case packageType::MASTERBOARD_DATA:
val_lock_.lock(); val_lock_.lock();
RobotState::unpackRobotStateMasterboard(buf, offset); RobotState::unpackRobotStateMasterboard(buf, offset+5);
val_lock_.unlock(); val_lock_.unlock();
break; break;
default: default:
@@ -104,6 +108,7 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
memcpy(&version_msg_.project_name, &buf[offset], memcpy(&version_msg_.project_name, &buf[offset],
sizeof(char) * version_msg_.project_name_size); sizeof(char) * version_msg_.project_name_size);
offset += 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], memcpy(&version_msg_.major_version, &buf[offset],
sizeof(version_msg_.major_version)); sizeof(version_msg_.major_version));
offset += sizeof(version_msg_.major_version); offset += sizeof(version_msg_.major_version);
@@ -113,7 +118,9 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
memcpy(&version_msg_.svn_revision, &buf[offset], memcpy(&version_msg_.svn_revision, &buf[offset],
sizeof(version_msg_.svn_revision)); sizeof(version_msg_.svn_revision));
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); memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
version_msg_.build_date[len - offset] = '\0';
} }
void RobotState::unpackRobotStateMasterboard(uint8_t * buf, void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
@@ -124,50 +131,61 @@ void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
offset += sizeof(digital_input_bits); offset += sizeof(digital_input_bits);
memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits)); memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
offset += sizeof(digital_output_bits); offset += sizeof(digital_output_bits);
mb_data_.digitalInputBits = digital_input_bits; mb_data_.digitalInputBits = ntohs(digital_input_bits);
mb_data_.digitalOutputBits = digital_output_bits; mb_data_.digitalOutputBits = ntohs(digital_output_bits);
} else { } else {
memcpy(&mb_data_.digitalInputBits, &buf[offset], memcpy(&mb_data_.digitalInputBits, &buf[offset],
sizeof(mb_data_.digitalInputBits)); sizeof(mb_data_.digitalInputBits));
offset += sizeof(mb_data_.digitalInputBits); offset += sizeof(mb_data_.digitalInputBits);
mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
memcpy(&mb_data_.digitalOutputBits, &buf[offset], memcpy(&mb_data_.digitalOutputBits, &buf[offset],
sizeof(mb_data_.digitalOutputBits)); sizeof(mb_data_.digitalOutputBits));
offset += sizeof(mb_data_.digitalOutputBits); offset += sizeof(mb_data_.digitalOutputBits);
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
} }
printf("output bits: %X\n", mb_data_.digitalOutputBits);
memcpy(&mb_data_.analogInputRange0, &buf[offset], memcpy(&mb_data_.analogInputRange0, &buf[offset],
sizeof(mb_data_.analogInputRange0)); sizeof(mb_data_.analogInputRange0));
offset += sizeof(mb_data_.analogInputRange0); offset += sizeof(mb_data_.analogInputRange0);
memcpy(&mb_data_.analogInputRange1, &buf[offset], memcpy(&mb_data_.analogInputRange1, &buf[offset],
sizeof(mb_data_.analogInputRange1)); sizeof(mb_data_.analogInputRange1));
offset += sizeof(mb_data_.analogInputRange1); offset += sizeof(mb_data_.analogInputRange1);
memcpy(&mb_data_.analogInput0, &buf[offset], sizeof(mb_data_.analogInput0)); uint64_t temp;
offset += sizeof(mb_data_.analogInput0); memcpy(&temp, &buf[offset], sizeof(temp));
memcpy(&mb_data_.analogInput1, &buf[offset], sizeof(mb_data_.analogInput1)); offset += sizeof(temp);
offset += sizeof(mb_data_.analogInput1); 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], memcpy(&mb_data_.analogOutputDomain0, &buf[offset],
sizeof(mb_data_.analogOutputDomain0)); sizeof(mb_data_.analogOutputDomain0));
offset += sizeof(mb_data_.analogOutputDomain0); offset += sizeof(mb_data_.analogOutputDomain0);
memcpy(&mb_data_.analogOutputDomain1, &buf[offset], memcpy(&mb_data_.analogOutputDomain1, &buf[offset],
sizeof(mb_data_.analogOutputDomain1)); sizeof(mb_data_.analogOutputDomain1));
offset += sizeof(mb_data_.analogOutputDomain1); offset += sizeof(mb_data_.analogOutputDomain1);
memcpy(&mb_data_.analogOutput0, &buf[offset], memcpy(&temp, &buf[offset], sizeof(temp));
sizeof(mb_data_.analogOutput0)); offset += sizeof(temp);
offset += sizeof(mb_data_.analogOutput0); mb_data_.analogOutput0 = RobotState::ntohd(temp);
memcpy(&mb_data_.analogOutput1, &buf[offset], memcpy(&temp, &buf[offset], sizeof(temp));
sizeof(mb_data_.analogOutput1)); offset += sizeof(temp);
offset += sizeof(mb_data_.analogOutput1); mb_data_.analogOutput1 = RobotState::ntohd(temp);
memcpy(&mb_data_.masterBoardTemperature, &buf[offset], memcpy(&mb_data_.masterBoardTemperature, &buf[offset],
sizeof(mb_data_.masterBoardTemperature)); sizeof(mb_data_.masterBoardTemperature));
offset += sizeof(mb_data_.masterBoardTemperature); offset += sizeof(mb_data_.masterBoardTemperature);
mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
memcpy(&mb_data_.robotVoltage48V, &buf[offset], memcpy(&mb_data_.robotVoltage48V, &buf[offset],
sizeof(mb_data_.robotVoltage48V)); sizeof(mb_data_.robotVoltage48V));
offset += sizeof(mb_data_.robotVoltage48V); offset += sizeof(mb_data_.robotVoltage48V);
mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent)); memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
offset += sizeof(mb_data_.robotCurrent); offset += sizeof(mb_data_.robotCurrent);
mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
memcpy(&mb_data_.masterIOCurrent, &buf[offset], memcpy(&mb_data_.masterIOCurrent, &buf[offset],
sizeof(mb_data_.masterIOCurrent)); sizeof(mb_data_.masterIOCurrent));
offset += sizeof(mb_data_.masterIOCurrent); offset += sizeof(mb_data_.masterIOCurrent);
mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode)); memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
offset += sizeof(mb_data_.safetyMode); offset += sizeof(mb_data_.safetyMode);
@@ -182,24 +200,28 @@ void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
memcpy(&mb_data_.euromapInputBits, &buf[offset], memcpy(&mb_data_.euromapInputBits, &buf[offset],
sizeof(mb_data_.euromapInputBits)); sizeof(mb_data_.euromapInputBits));
offset += sizeof(mb_data_.euromapInputBits); offset += sizeof(mb_data_.euromapInputBits);
mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
memcpy(&mb_data_.euromapOutputBits, &buf[offset], memcpy(&mb_data_.euromapOutputBits, &buf[offset],
sizeof(mb_data_.euromapOutputBits)); sizeof(mb_data_.euromapOutputBits));
offset += sizeof(mb_data_.euromapOutputBits); offset += sizeof(mb_data_.euromapOutputBits);
mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
if (RobotState::getVersion() < 3.0) { if (RobotState::getVersion() < 3.0) {
int16_t euromap_voltage, euromap_current; int16_t euromap_voltage, euromap_current;
memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage)); memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
offset += sizeof(euromap_voltage); offset += sizeof(euromap_voltage);
memcpy(&euromap_current, &buf[offset], sizeof(euromap_current)); memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
offset += sizeof(euromap_current); offset += sizeof(euromap_current);
mb_data_.euromapVoltage = euromap_voltage; mb_data_.euromapVoltage = ntohs(euromap_voltage);
mb_data_.euromapCurrent = euromap_current; mb_data_.euromapCurrent = ntohs(euromap_current);
} else { } else {
memcpy(&mb_data_.euromapVoltage, &buf[offset], memcpy(&mb_data_.euromapVoltage, &buf[offset],
sizeof(mb_data_.euromapVoltage)); sizeof(mb_data_.euromapVoltage));
offset += sizeof(mb_data_.euromapVoltage); offset += sizeof(mb_data_.euromapVoltage);
mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
memcpy(&mb_data_.euromapCurrent, &buf[offset], memcpy(&mb_data_.euromapCurrent, &buf[offset],
sizeof(mb_data_.euromapCurrent)); sizeof(mb_data_.euromapCurrent));
offset += sizeof(mb_data_.euromapCurrent); offset += sizeof(mb_data_.euromapCurrent);
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
} }
} }
@@ -219,6 +241,10 @@ void RobotState::finishedReading() {
new_data_available_ = false; new_data_available_ = false;
} }
bool RobotState::getNewDataAvailable() {
return new_data_available_;
}
int RobotState::getDigitalInputBits() { int RobotState::getDigitalInputBits() {
return mb_data_.digitalInputBits; return mb_data_.digitalInputBits;
} }

2
src/robot_state_RT.cpp
View File

@@ -314,7 +314,7 @@ void RobotStateRT::unpack(uint8_t * buf) {
} }
offset += 4; offset += 4;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to)); memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = ntohd(unpack_to); time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double); offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 6); q_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6; offset += sizeof(double) * 6;

33
src/ur_communication.cpp
View File

@@ -16,8 +16,13 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
robot_state_ = new RobotState(msg_cond); robot_state_ = new RobotState(msg_cond);
bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_)); bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0); pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) { if (pri_sockfd_ < 0) {
printf("ERROR opening socket");
exit(0);
}
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0) {
printf("ERROR opening socket"); printf("ERROR opening socket");
exit(0); exit(0);
} }
@@ -33,8 +38,10 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
pri_serv_addr_.sin_port = htons(30001); pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002); sec_serv_addr_.sin_port = htons(30002);
flag_ = 1; flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (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_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
connected_ = false; connected_ = false;
keepalive_ = false; keepalive_ = false;
} }
@@ -47,22 +54,22 @@ void UrCommunication::start() {
bzero(buf, 512); bzero(buf, 512);
printf("Acquire firmware version: Connecting...\n"); printf("Acquire firmware version: Connecting...\n");
if (connect(sockfd_, (struct sockaddr *) &pri_serv_addr_, if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
sizeof(pri_serv_addr_)) < 0) { sizeof(pri_serv_addr_)) < 0) {
printf("Error connecting"); printf("Error connecting");
exit(1); exit(1);
} }
printf("Acquire firmware version: Got connection\n"); printf("Acquire firmware version: Got connection\n");
bytes_read = read(sockfd_, buf, 512); bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read); robot_state_->unpack(buf, bytes_read);
//wait for some traffic so the UR socket doesn't die in version 3.1. //wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));
printf("Firmware version: %f\n\n", robot_state_->getVersion()); printf("Firmware version: %f\n\n", robot_state_->getVersion());
close(sockfd_); close(pri_sockfd_);
printf("Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic printf("Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
if (connect(sockfd_, (struct sockaddr *) &sec_serv_addr_, if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_)) < 0) { sizeof(sec_serv_addr_)) < 0) {
printf("Error connecting"); printf("Error connecting");
exit(1); exit(1);
@@ -92,13 +99,13 @@ void UrCommunication::run() {
printf("Got connection\n"); printf("Got connection\n");
connected_ = true; connected_ = true;
while (keepalive_) { while (keepalive_) {
bytes_read = read(sockfd_, buf, 2048); // usually only up to 1295 bytes bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int)); sizeof(int));
robot_state_->unpack(buf, bytes_read); robot_state_->unpack(buf, bytes_read);
command_string_lock_.lock(); command_string_lock_.lock();
if (command_.length() != 0) { if (command_.length() != 0) {
write(sockfd_, command_.c_str(), command_.length()); write(sec_sockfd_, command_.c_str(), command_.length());
command_ = ""; command_ = "";
} }
command_string_lock_.unlock(); command_string_lock_.unlock();
@@ -106,6 +113,6 @@ void UrCommunication::run() {
} }
//wait for some traffic so the UR socket doesn't die in version 3.1. //wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sockfd_); close(sec_sockfd_);
} }

2
src/ur_driver.cpp
View File

@@ -16,7 +16,7 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable
double max_payload) : double max_payload) :
maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_( maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_(
max_payload) { max_payload) {
rt_interface_ = new UrRealtimeCommunication(msg_cond, host, rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max); safety_count_max);
sec_interface_ = new UrCommunication(msg_cond, host); sec_interface_ = new UrCommunication(msg_cond, host);

9
src/ur_ros_wrapper.cpp
View File

@@ -51,6 +51,7 @@ protected:
ros::ServiceServer io_srv_; ros::ServiceServer io_srv_;
ros::ServiceServer payload_srv_; ros::ServiceServer payload_srv_;
std::thread* rt_publish_thread_; std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_;
double io_flag_delay_; double io_flag_delay_;
double max_velocity_; double max_velocity_;
std::vector<double> joint_offsets_; std::vector<double> joint_offsets_;
@@ -111,7 +112,7 @@ public:
//subscribe to the data topic of interest //subscribe to the data topic of interest
speed_sub_ = nh_.subscribe("joint_speed", 1, speed_sub_ = nh_.subscribe("joint_speed", 1,
&RosWrapper::speedInterface, this); &RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URSCcript", 1, urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
&RosWrapper::urscriptInterface, this); &RosWrapper::urscriptInterface, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO,
@@ -121,6 +122,8 @@ public:
rt_publish_thread_ = new std::thread( rt_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishRTMsg, this)); boost::bind(&RosWrapper::publishRTMsg, this));
mb_publish_thread_ = new std::thread(
boost::bind(&RosWrapper::publishMbMsg, this));
ROS_INFO("The action server for this driver has been started"); ROS_INFO("The action server for this driver has been started");
} }
@@ -369,7 +372,7 @@ private:
} }
} }
void publishMsg() { void publishMbMsg() {
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("/io_states", ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("/io_states",
1); 1);
@@ -381,7 +384,7 @@ private:
msg_cond_.wait(locker); msg_cond_.wait(locker);
} }
for (unsigned int i = 0; i < 10; i++) { for (unsigned int i = 0; i < 18; i++) {
ur_msgs::Digital digi; ur_msgs::Digital digi;
digi.pin = i; digi.pin = i;
digi.state = digi.state =