ur_realtime_communication.cpp
Go to the documentation of this file.
00001 /*
00002  * ur_realtime_communication.cpp
00003  *
00004  * Copyright 2015 Thomas Timm Andersen
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  *     http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #include "ur_modern_driver/ur_realtime_communication.h"
00020 
00021 UrRealtimeCommunication::UrRealtimeCommunication(
00022                 std::condition_variable& msg_cond, std::string host,
00023                 unsigned int safety_count_max) {
00024         robot_state_ = new RobotStateRT(msg_cond);
00025         bzero((char *) &serv_addr_, sizeof(serv_addr_));
00026         sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
00027         if (sockfd_ < 0) {
00028                 print_fatal("ERROR opening socket");
00029         }
00030         server_ = gethostbyname(host.c_str());
00031         if (server_ == NULL) {
00032                 print_fatal("ERROR, no such host");
00033         }
00034         serv_addr_.sin_family = AF_INET;
00035         bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
00036         serv_addr_.sin_port = htons(30003);
00037         flag_ = 1;
00038         setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
00039         setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, sizeof(int));
00040         setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
00041         fcntl(sockfd_, F_SETFL, O_NONBLOCK);
00042         connected_ = false;
00043         keepalive_ = false;
00044         safety_count_ = safety_count_max + 1;
00045         safety_count_max_ = safety_count_max;
00046 }
00047 
00048 bool UrRealtimeCommunication::start() {
00049         fd_set writefds;
00050         struct timeval timeout;
00051 
00052         keepalive_ = true;
00053         print_debug("Realtime port: Connecting...");
00054 
00055         connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
00056         FD_ZERO(&writefds);
00057         FD_SET(sockfd_, &writefds);
00058         timeout.tv_sec = 10;
00059         timeout.tv_usec = 0;
00060         select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
00061         unsigned int flag_len;
00062         getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
00063         if (flag_ < 0) {
00064                 print_fatal("Error connecting to RT port 30003");
00065                 return false;
00066         }
00067         sockaddr_in name;
00068         socklen_t namelen = sizeof(name);
00069         int err = getsockname(sockfd_, (sockaddr*) &name, &namelen);
00070         if (err < 0) {
00071                 print_fatal("Could not get local IP");
00072                 close(sockfd_);
00073                 return false;
00074         }
00075         char str[18];
00076         inet_ntop(AF_INET, &name.sin_addr, str, 18);
00077         local_ip_ = str;
00078         comThread_ = std::thread(&UrRealtimeCommunication::run, this);
00079         return true;
00080 }
00081 
00082 void UrRealtimeCommunication::halt() {
00083         keepalive_ = false;
00084         comThread_.join();
00085 }
00086 
00087 void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
00088         int bytes_written;
00089         if (inp.back() != '\n') {
00090                 inp.append("\n");
00091         }
00092         if (connected_)
00093                 bytes_written = write(sockfd_, inp.c_str(), inp.length());
00094         else
00095                 print_error("Could not send command \"" +inp + "\". The robot is not connected! Command is discarded" );
00096 }
00097 
00098 void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
00099                 double q3, double q4, double q5, double acc) {
00100         char cmd[1024];
00101         if( robot_state_->getVersion() >= 3.3 ) {
00102                 sprintf(cmd,
00103                                 "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.008)\n",
00104                                 q0, q1, q2, q3, q4, q5, acc);
00105         }
00106         else if( robot_state_->getVersion() >= 3.1 ) {
00107                 sprintf(cmd,
00108                                 "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
00109                                 q0, q1, q2, q3, q4, q5, acc);
00110         }
00111         else {
00112                 sprintf(cmd,
00113                                 "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n",
00114                                 q0, q1, q2, q3, q4, q5, acc);           
00115         }
00116         addCommandToQueue((std::string) (cmd));
00117         if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
00118                 //If a joint speed is set, make sure we stop it again after some time if the user doesn't
00119                 safety_count_ = 0;
00120         }
00121 }
00122 
00123 void UrRealtimeCommunication::run() {
00124         uint8_t buf[2048];
00125         int bytes_read;
00126         bzero(buf, 2048);
00127         struct timeval timeout;
00128         fd_set readfds;
00129         FD_ZERO(&readfds);
00130         FD_SET(sockfd_, &readfds);
00131         print_debug("Realtime port: Got connection");
00132         connected_ = true;
00133         while (keepalive_) {
00134                 while (connected_ && keepalive_) {
00135                         timeout.tv_sec = 0; //do this each loop as selects modifies timeout
00136                         timeout.tv_usec = 500000; // timeout of 0.5 sec
00137                         select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
00138                         bytes_read = read(sockfd_, buf, 2048);
00139                         if (bytes_read > 0) {
00140                                 setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, 
00141                                                 sizeof(int));
00142                                 robot_state_->unpack(buf);
00143                                 if (safety_count_ == safety_count_max_) {
00144                                         setSpeed(0., 0., 0., 0., 0., 0.);
00145                                 }
00146                                 safety_count_ += 1;
00147                         } else {
00148                                 connected_ = false;
00149                                 close(sockfd_);
00150                         }
00151                 }
00152                 if (keepalive_) {
00153                         //reconnect
00154                         print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
00155                         sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
00156                         if (sockfd_ < 0) {
00157                                 print_fatal("ERROR opening socket");
00158                         }
00159                         flag_ = 1;
00160                         setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
00161                                         sizeof(int));
00162                         setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char *) &flag_, 
00163                                         sizeof(int));
00164         
00165                         setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
00166                                         sizeof(int));
00167                         fcntl(sockfd_, F_SETFL, O_NONBLOCK);
00168                         while (keepalive_ && !connected_) {
00169                                 std::this_thread::sleep_for(std::chrono::seconds(10));
00170                                 fd_set writefds;
00171 
00172                                 connect(sockfd_, (struct sockaddr *) &serv_addr_,
00173                                                 sizeof(serv_addr_));
00174                                 FD_ZERO(&writefds);
00175                                 FD_SET(sockfd_, &writefds);
00176                                 select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
00177                                 unsigned int flag_len;
00178                                 getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
00179                                 if (flag_ < 0) {
00180                                         print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
00181                                 } else {
00182                                         connected_ = true;
00183                                         print_info("Realtime port: Reconnected");
00184                                 }
00185                         }
00186                 }
00187         }
00188         setSpeed(0., 0., 0., 0., 0., 0.);
00189         close(sockfd_);
00190 }
00191 
00192 void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
00193         safety_count_max_ = inp;
00194 }
00195 
00196 std::string UrRealtimeCommunication::getLocalIp() {
00197         return local_ip_;
00198 }


ur_modern_driver
Author(s): Thomas Timm Andersen
autogenerated on Wed Apr 3 2019 02:55:31