From 6e9f646b02d55b8f7e32c1447cf40a78a7a6efc2 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 16:10:33 +0200 Subject: [PATCH] Added reconnect to secondary port --- include/ur_modern_driver/ur_communication.h | 4 +- src/ur_communication.cpp | 104 +++++++++++++++++--- src/ur_driver.cpp | 23 ++--- 3 files changed, 107 insertions(+), 24 deletions(-) diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index c9bc361..f9c1b45 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include #ifdef ROS_BUILD #include @@ -49,7 +51,7 @@ public: RobotState* robot_state_; UrCommunication(std::condition_variable& msg_cond, std::string host); - void start(); + bool start(); void halt(); }; diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 5b2a27f..502cb6f 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -61,11 +61,12 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond, sizeof(int)); setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); connected_ = false; keepalive_ = false; } -void UrCommunication::start() { +bool UrCommunication::start() { keepalive_ = true; uint8_t buf[512]; unsigned int bytes_read; @@ -81,9 +82,10 @@ void UrCommunication::start() { #ifdef ROS_BUILD ROS_FATAL("Error connecting"); ros::shutdown(); + return false; #else printf("Error connecting\n"); - exit(1); + return false; #endif } #ifdef ROS_BUILD @@ -110,14 +112,27 @@ void UrCommunication::start() { printf( "Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic #endif - if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)) < 0) { + + 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) { #ifdef ROS_BUILD - ROS_FATAL("Error connecting"); + ROS_FATAL("Error connecting to secondary interface"); ros::shutdown(); + return false; #else - printf("Error connecting\n"); - exit(1); + printf("Error connecting to secondary interface\n"); + return false; #endif } #ifdef ROS_BUILD @@ -126,6 +141,7 @@ void UrCommunication::start() { printf("Secondary interface: Got connection\n"); #endif comThread_ = std::thread(&UrCommunication::run, this); + return true; } void UrCommunication::halt() { @@ -135,15 +151,79 @@ void UrCommunication::halt() { void UrCommunication::run() { uint8_t buf[2048]; - unsigned int bytes_read; + int bytes_read; bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sec_sockfd_, &readfds); connected_ = true; while (keepalive_) { - 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); + 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_NODELAY, + (char *) &flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + } else { + connected_ = false; + close (sec_sockfd_); + } + } + if (keepalive_) { + //reconnect +#ifdef ROS_BUILD + ROS_WARN("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); +#else + printf( + "Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n"); +#endif + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening secondary socket"); + ros::shutdown(); +#else + printf("ERROR opening secondary socket"); + exit(1); +#endif + } + flag_ = 1; + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (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; + keepalive_ = true; + + 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) { +#ifdef ROS_BUILD + ROS_ERROR("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); +#else + printf( + "Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); +#endif + } else { + connected_ = true; + } + } + } } + //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_); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 382f7ec..9a7bdd6 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -91,9 +91,9 @@ void UrDriver::addTraj(std::vector inp_timestamps, } //make sure we come to a smooth stop /*while (timestamps.back() < inp_timestamps.back()) { - timestamps.push_back(timestamps.back() + 0.008); - } - timestamps.pop_back();*/ + timestamps.push_back(timestamps.back() + 0.008); + } + timestamps.pop_back();*/ unsigned int j = 0; for (unsigned int i = 0; i < timestamps.size(); i++) { @@ -162,7 +162,7 @@ void UrDriver::doTraj(std::vector inp_timestamps, } positions = UrDriver::interp_cubic( std::chrono::duration_cast>( - t - t0).count()- inp_timestamps[j - 1], + 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]); for (int i = 0; i < 6; i++) { @@ -297,16 +297,17 @@ void UrDriver::uploadProg() { } void UrDriver::start() { - sec_interface_->start(); - rt_interface_->robot_state_->setVersion( - sec_interface_->robot_state_->getVersion()); - rt_interface_->start(); - ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); + if (sec_interface_->start()) { + rt_interface_->robot_state_->setVersion( + sec_interface_->robot_state_->getVersion()); + rt_interface_->start(); + ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); #ifdef ROS_BUILD - ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); + ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); #else - printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); + printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); #endif + } } void UrDriver::halt() {