1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00

Added catch in case communication fails

This commit is contained in:
Thomas Timm Andersen
2015-09-24 16:26:19 +02:00
parent ef7acd55b6
commit 6f3bd5d06f
5 changed files with 49 additions and 37 deletions

View File

@@ -49,7 +49,7 @@ UrRealtimeCommunication::UrRealtimeCommunication(
safety_count_max_ = safety_count_max;
}
void UrRealtimeCommunication::start() {
bool UrRealtimeCommunication::start() {
fd_set writefds;
struct timeval timeout;
@@ -71,10 +71,12 @@ void UrRealtimeCommunication::start() {
if (flag_ < 0) {
#ifdef ROS_BUILD
ROS_FATAL("Error connecting to RT port 30003 - errno: %d (%s)", flag_, strerror(flag_));
#else
printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_,
strerror(flag_));
#endif
return false;
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
@@ -86,11 +88,14 @@ void UrRealtimeCommunication::start() {
printf("Could not get local IP - errno: %d (%s)", errno,
strerror(errno));
#endif
close(sockfd_);
return false;
}
char str[18];
inet_ntop(AF_INET, &name.sin_addr, str, 18);
local_ip_ = str;
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
return true;
}
void UrRealtimeCommunication::halt() {