ur_realtime_communication.cpp

Go to the documentation of this file.

/*
 * ur_realtime_communication.cpp
 *
 * Copyright 2015 Thomas Timm Andersen
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ur_modern_driver/ur_realtime_communication.h"

UrRealtimeCommunication::UrRealtimeCommunication(
                std::condition_variable& msg_cond, std::string host,
                unsigned int safety_count_max) {
        robot_state_ = new RobotStateRT(msg_cond);
        bzero((char *) &serv_addr_, sizeof(serv_addr_));
        sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
        if (sockfd_ < 0) {
                print_fatal("ERROR opening socket");
        }
        server_ = gethostbyname(host.c_str());
        if (server_ == NULL) {
                print_fatal("ERROR, no such host");
        }
        serv_addr_.sin_family = AF_INET;
        bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
        serv_addr_.sin_port = htons(30003);
        flag_ = 1;
        setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
        setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (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;
        safety_count_max_ = safety_count_max;
}

bool UrRealtimeCommunication::start() {
        fd_set writefds;
        struct timeval timeout;

        keepalive_ = true;
        print_debug("Realtime port: Connecting...");

        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) {
                print_fatal("Error connecting to RT port 30003");
                return false;
        }
        sockaddr_in name;
        socklen_t namelen = sizeof(name);
        int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
        if (err < 0) {
                print_fatal("Could not get local IP");
                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() {
        keepalive_ = false;
        comThread_.join();
}

void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
        int bytes_written;
        if (inp.back() != '\n') {
                inp.append("\n");
        }
        if (connected_)
                bytes_written = write(sockfd_, inp.c_str(), inp.length());
        else
                print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" );
}

void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
                double q3, double q4, double q5, double acc) {
        char cmd[1024];
        if( robot_state_->getVersion() >= 3.3 ) {
                sprintf(cmd,
                                "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.008)\n",
                                q0, q1, q2, q3, q4, q5, acc);
        }
        else if( robot_state_->getVersion() >= 3.1 ) {
                sprintf(cmd,
                                "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
                                q0, q1, q2, q3, q4, q5, acc);
        }
        else {
                sprintf(cmd,
                                "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
                                q0, q1, q2, q3, q4, q5, acc);           
        }
        addCommandToQueue((std::string) (cmd));
        if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
                //If a joint speed is set, make sure we stop it again after some time if the user doesn't
                safety_count_ = 0;
        }
}

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);
        print_debug("Realtime port: Got connection");
        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(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
                        bytes_read = read(sockfd_, buf, 2048);
                        if (bytes_read > 0) {
                                setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (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
                        print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
                        sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
                        if (sockfd_ < 0) {
                                print_fatal("ERROR opening socket");
                        }
                        flag_ = 1;
                        setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
                                        sizeof(int));
                        setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (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;

                                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) {
                                        print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
                                } else {
                                        connected_ = true;
                                        print_info("Realtime port: Reconnected");
                                }
                        }
                }
        }
        setSpeed(0., 0., 0., 0., 0., 0.);
        close(sockfd_);
}

void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
        safety_count_max_ = inp;
}

std::string UrRealtimeCommunication::getLocalIp() {
        return local_ip_;
}