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

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

76
src/robot_state.cpp
View File

@@ -13,19 +13,24 @@ RobotState::RobotState(std::condition_variable& msg_cond) {
new_data_available_ = false;
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) {
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
unsigned int offset = 0;
while (buf_length > offset) {
uint32_t len;
int len;
unsigned char message_type;
memcpy(&len, &buf[offset], sizeof(len));
len = ntohl(len);
if (len + offset > buf_length) {
return (len + offset - buf_length);
}
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
@@ -58,7 +63,7 @@ void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
switch (robot_message_type) {
case robotMessageType::ROBOT_MESSAGE_VERSION:
val_lock_.lock();
version_msg_.timestamp = timestamp;
version_msg_.timestamp = RobotState::ntohd(timestamp);
version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len);
@@ -74,16 +79,15 @@ void RobotState::unpackRobotState(uint8_t * buf, unsigned int offset,
uint32_t len) {
offset += 5;
while (offset < len) {
uint32_t length;
int8_t package_type;
int32_t length;
uint8_t package_type;
memcpy(&length, &buf[offset], sizeof(length));
offset += sizeof(length);
memcpy(&package_type, &buf[offset], sizeof(package_type));
offset += sizeof(package_type);
length = ntohl(length);
memcpy(&package_type, &buf[offset+sizeof(length)], sizeof(package_type));
switch (package_type) {
case packageType::MASTERBOARD_DATA:
val_lock_.lock();
RobotState::unpackRobotStateMasterboard(buf, offset);
RobotState::unpackRobotStateMasterboard(buf, offset+5);
val_lock_.unlock();
break;
default:
@@ -104,6 +108,7 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
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);
@@ -113,61 +118,74 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
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';
}
void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
unsigned int offset) {
if (RobotState::getVersion() < 3.0) {
int16_t digital_input_bits, digital_output_bits;
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 = digital_input_bits;
mb_data_.digitalOutputBits = 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);
}
printf("output bits: %X\n", 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);
memcpy(&mb_data_.analogInput0, &buf[offset], sizeof(mb_data_.analogInput0));
offset += sizeof(mb_data_.analogInput0);
memcpy(&mb_data_.analogInput1, &buf[offset], sizeof(mb_data_.analogInput1));
offset += sizeof(mb_data_.analogInput1);
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(&mb_data_.analogOutput0, &buf[offset],
sizeof(mb_data_.analogOutput0));
offset += sizeof(mb_data_.analogOutput0);
memcpy(&mb_data_.analogOutput1, &buf[offset],
sizeof(mb_data_.analogOutput1));
offset += sizeof(mb_data_.analogOutput1);
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);
@@ -182,24 +200,28 @@ void RobotState::unpackRobotStateMasterboard(uint8_t * buf,
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 = euromap_voltage;
mb_data_.euromapCurrent = 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);
}
}
@@ -219,6 +241,10 @@ void RobotState::finishedReading() {
new_data_available_ = false;
}
bool RobotState::getNewDataAvailable() {
return new_data_available_;
}
int RobotState::getDigitalInputBits() {
return mb_data_.digitalInputBits;
}

2
src/robot_state_RT.cpp
View File

@@ -314,7 +314,7 @@ void RobotStateRT::unpack(uint8_t * buf) {
}
offset += 4;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = ntohd(unpack_to);
time_ = RobotStateRT::ntohd(unpack_to);
offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 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);
bzero((char *) &pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 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");
exit(0);
}
@@ -33,8 +38,10 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (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;
keepalive_ = false;
}
@@ -47,22 +54,22 @@ void UrCommunication::start() {
bzero(buf, 512);
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) {
printf("Error connecting");
exit(1);
}
printf("Acquire firmware version: Got connection\n");
bytes_read = read(sockfd_, buf, 512);
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (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));
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
if (connect(sockfd_, (struct sockaddr *) &sec_serv_addr_,
if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_,
sizeof(sec_serv_addr_)) < 0) {
printf("Error connecting");
exit(1);
@@ -92,13 +99,13 @@ void UrCommunication::run() {
printf("Got connection\n");
connected_ = true;
while (keepalive_) {
bytes_read = read(sockfd_, buf, 2048); // usually only up to 1295 bytes
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf, bytes_read);
command_string_lock_.lock();
if (command_.length() != 0) {
write(sockfd_, command_.c_str(), command_.length());
write(sec_sockfd_, command_.c_str(), command_.length());
command_ = "";
}
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.
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) :
maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_(
max_payload) {
rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max);
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 payload_srv_;
std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_;
double io_flag_delay_;
double max_velocity_;
std::vector<double> joint_offsets_;
@@ -111,7 +112,7 @@ public:
//subscribe to the data topic of interest
speed_sub_ = nh_.subscribe("joint_speed", 1,
&RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URSCcript", 1,
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
&RosWrapper::urscriptInterface, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO,
@@ -121,6 +122,8 @@ public:
rt_publish_thread_ = new std::thread(
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");
}
@@ -369,7 +372,7 @@ private:
}
}
void publishMsg() {
void publishMbMsg() {
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("/io_states",
1);
@@ -381,7 +384,7 @@ private:
msg_cond_.wait(locker);
}
for (unsigned int i = 0; i < 10; i++) {
for (unsigned int i = 0; i < 18; i++) {
ur_msgs::Digital digi;
digi.pin = i;
digi.state =