From 7c6080e89e91def24d3b3951f83fe742566424bc Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 15:36:54 +0200 Subject: [PATCH] Added reconnect feature on RT port --- include/ur_modern_driver/robot_state_RT.h | 9 +- .../ur_realtime_communication.h | 2 + src/robot_state_RT.cpp | 25 +++-- src/ur_communication.cpp | 4 +- src/ur_realtime_communication.cpp | 100 +++++++++++++++--- src/ur_ros_wrapper.cpp | 4 +- 6 files changed, 118 insertions(+), 26 deletions(-) diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 77823f1..bec0767 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -56,7 +56,8 @@ private: std::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 + bool data_published_; //to avoid spurious wakes + bool controller_updated_; //to avoid spurious wakes std::vector unpackVector(uint8_t * buf, int start_index, int nr_of_vals); @@ -94,11 +95,13 @@ public: double getVMain(); double getVRobot(); double getIRobot(); - bool getNewDataAvailable(); void setVersion(double ver); - void finishedReading(); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); std::vector getVActual(); void unpack(uint8_t * buf); }; diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 2e43e43..2a8be1f 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -30,6 +30,8 @@ #include #include #include +#include +#include #ifdef ROS_BUILD #include diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 602f310..273db0e 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -41,23 +41,33 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { v_robot_ = 0.0; i_robot_ = 0.0; v_actual_.assign(6, 0.0); - new_data_available_ = false; + data_published_ = false; + controller_updated_ = false; pMsg_cond_ = &msg_cond; } RobotStateRT::~RobotStateRT() { /* Make sure nobody is waiting after this thread is destroyed */ - new_data_available_ = true; + data_published_ = true; + controller_updated_ = true; pMsg_cond_->notify_all(); } -bool RobotStateRT::getNewDataAvailable() { - return new_data_available_; + +void RobotStateRT::setDataPublished() { + data_published_ = false; +} +bool RobotStateRT::getDataPublished() { + return data_published_; } -void RobotStateRT::finishedReading() { - new_data_available_ = false; +void RobotStateRT::setControllerUpdated() { + controller_updated_ = false; } +bool RobotStateRT::getControllerUpdated() { + return controller_updated_; +} + double RobotStateRT::ntohd(uint64_t nf) { double x; @@ -394,7 +404,8 @@ void RobotStateRT::unpack(uint8_t * buf) { v_actual_ = unpackVector(buf, offset, 6); } val_lock_.unlock(); - new_data_available_ = true; + controller_updated_ = true; + data_published_ = true; pMsg_cond_->notify_all(); } diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 5b2a27f..5b406ec 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -79,10 +79,10 @@ void UrCommunication::start() { if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, sizeof(pri_serv_addr_)) < 0) { #ifdef ROS_BUILD - ROS_FATAL("Error connecting"); + ROS_FATAL("Error connecting to get firmware version"); ros::shutdown(); #else - printf("Error connecting\n"); + printf("Error connecting to get firmware version\n"); exit(1); #endif } diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 23ecfd2..ebb2e6e 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -42,6 +42,7 @@ UrRealtimeCommunication::UrRealtimeCommunication( flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (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; @@ -49,18 +50,30 @@ UrRealtimeCommunication::UrRealtimeCommunication( } void UrRealtimeCommunication::start() { + fd_set writefds; + struct timeval timeout; + keepalive_ = true; #ifdef ROS_BUILD ROS_DEBUG("Realtime port: Connecting..."); #else printf("Realtime port: Connecting...\n"); #endif - if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)) - < 0) { + + 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) { #ifdef ROS_BUILD - ROS_FATAL("Error connecting to RT port 30003"); + ROS_FATAL("Error connecting to RT port 30003 - errno: %d (%s)", flag_, strerror(flag_)); #else - printf("Error connecting to RT port 30003\n"); + printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_, + strerror(flag_)); #endif } sockaddr_in name; @@ -70,7 +83,8 @@ void UrRealtimeCommunication::start() { #ifdef ROS_BUILD ROS_FATAL("Could not get local IP - errno: %d (%s)", errno, strerror(errno)); #else - printf("Could not get local IP - errno: %d (%s)", errno, strerror(errno)); + printf("Could not get local IP - errno: %d (%s)", errno, + strerror(errno)); #endif } char str[18]; @@ -109,6 +123,10 @@ 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); #ifdef ROS_BUILD ROS_DEBUG("Realtime port: Got connection"); #else @@ -116,14 +134,72 @@ void UrRealtimeCommunication::run() { #endif connected_ = true; while (keepalive_) { - bytes_read = read(sockfd_, buf, 2048); - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) { - setSpeed(0., 0., 0., 0., 0., 0.); + 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_NODELAY, (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 +#ifdef ROS_BUILD + ROS_WARN("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); +#else + printf( + "Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n"); +#endif + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening socket"); + ros::shutdown(); +#else + printf("ERROR opening socket"); + exit(1); +#endif + } + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (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; + keepalive_ = true; + + 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) { +#ifdef ROS_BUILD + ROS_ERROR("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); +#else + printf( + "Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); +#endif + } else { + connected_ = true; + } + } } - safety_count_ += 1; } setSpeed(0., 0., 0., 0., 0., 0.); close(sockfd_); diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index e67c017..732e4f6 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -341,7 +341,7 @@ private: geometry_msgs::WrenchStamped wrench_msg; std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex std::unique_lock locker(msg_lock); - while (!robot_.rt_interface_->robot_state_->getNewDataAvailable()) { + while (!robot_.rt_interface_->robot_state_->getDataPublished()) { rt_msg_cond_.wait(locker); } joint_msg.header.stamp = ros::Time::now(); @@ -365,7 +365,7 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); - robot_.rt_interface_->robot_state_->finishedReading(); + robot_.rt_interface_->robot_state_->setDataPublished(); } }