Added reconnect to secondary port

include/ur_modern_driver/ur_communication.h

@@ -29,6 +29,8 @@
 #include <iostream>
 #include <unistd.h>
 #include <chrono>
+#include <fcntl.h>
+#include <sys/types.h>
 
 #ifdef ROS_BUILD
 #include <ros/ros.h>
@@ -49,7 +51,7 @@ public:
 	RobotState* robot_state_;
 
 	UrCommunication(std::condition_variable& msg_cond, std::string host);
-	void start();
+	bool start();
 	void halt();
 
 };

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_) {
+		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));
-		robot_state_->unpack(buf, bytes_read);
+			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_);

src/ur_driver.cpp

@@ -162,7 +162,7 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 		}
 		positions = UrDriver::interp_cubic(
 				std::chrono::duration_cast<std::chrono::duration<double>>(
-						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,7 +297,7 @@ void UrDriver::uploadProg() {
 }
 
 void UrDriver::start() {
-	sec_interface_->start();
+	if (sec_interface_->start()) {
 		rt_interface_->robot_state_->setVersion(
 				sec_interface_->robot_state_->getVersion());
 		rt_interface_->start();
@@ -307,6 +307,7 @@ void UrDriver::start() {
 #else
 		printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
 #endif
+	}
 }
 
 void UrDriver::halt() {