Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

Added reconnect to secondary port

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-24 16:10:33 +02:00
parent 039664a1a3
commit 6e9f646b02
3 changed files with 107 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/ur_modern_driver/ur_communication.h
View File

@@ -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();
};

104
src/ur_communication.cpp
View File

@@ -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_);

23
src/ur_driver.cpp
View File

@@ -91,9 +91,9 @@ void UrDriver::addTraj(std::vector<double> 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<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,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() {