diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h index 1806423..e85cf23 100644 --- a/include/ur_modern_driver/robot_state.h +++ b/include/ur_modern_driver/robot_state.h @@ -14,6 +14,7 @@ #include #include #include +#include namespace message_types { enum message_type { @@ -58,11 +59,11 @@ struct version_message { int8_t source; int8_t robot_message_type; int8_t project_name_size; - char* project_name; + char project_name[15]; uint8_t major_version; uint8_t minor_version; int svn_revision; - char* build_date; + char build_date[25]; }; struct masterboard_data { @@ -95,7 +96,7 @@ private: version_message version_msg_; 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 bool new_data_available_; //to avoid spurious wakes diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index 4e09d75..bf97f7c 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -32,7 +32,7 @@ class UrCommunication { private: - int sockfd_; + int pri_sockfd_, sec_sockfd_; struct sockaddr_in pri_serv_addr_, sec_serv_addr_; struct hostent *server_; bool keepalive_; diff --git a/src/robot_state.cpp b/src/robot_state.cpp index 07104c0..0fcd777 100644 --- a/src/robot_state.cpp +++ b/src/robot_state.cpp @@ -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; } diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 42293cf..64f40aa 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -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; diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 52e9cff..aac4925 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -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_); } diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 3bc302e..06b553b 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -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); diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 6c37e6e..8de6ab6 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -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 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("/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 =