nav2remote.cpp
Go to the documentation of this file.
00001 /*
00002   Copyright (C) 2012 CrossWing Inc. www.crosswing.com
00003   All Rights Reserved.
00004 */
00005 
00006 #include <cstdlib>
00007 #include <cstdio>
00008 #include <cstring>
00009 #include <cmath>
00010 #include <stdexcept>
00011 #include <unistd.h>
00012 #include <sys/types.h>
00013 #include <sys/socket.h>
00014 #include <netdb.h>
00015 
00016 #include <nav2_driver/nav2remote.h>
00017 
00018 Nav2Remote::Nav2Remote( const char *host, int port)
00019     : line(NULL), lineLen(0), fd(-1)
00020 {
00021     if(port < 1 || port > 65535) throw std::invalid_argument("Invalid port");
00022 
00023     char service[6];
00024     sprintf(service, "%d", port);
00025 
00026     struct addrinfo *ai;
00027     struct addrinfo hints;
00028     memset(&hints, 0, sizeof(hints));
00029     hints.ai_family = AF_UNSPEC;
00030     hints.ai_socktype = SOCK_STREAM;
00031     int s = getaddrinfo( host, service, &hints, &ai);
00032     if( s != 0) throw std::runtime_error("Can't get address info");
00033 
00034     for( struct addrinfo* rp = ai;; rp = rp->ai_next) {
00035         if( rp == NULL) {
00036             freeaddrinfo( ai);
00037             throw std::runtime_error("Can't connect to robot");
00038         }
00039 
00040         fd = socket( rp->ai_family, rp->ai_socktype, rp->ai_protocol);
00041         if( fd == -1) continue;
00042         if( connect(fd, rp->ai_addr, rp->ai_addrlen) == 0) break;
00043         close(fd);
00044         fd = -1;
00045     }
00046 
00047     freeaddrinfo(ai);
00048 }
00049 
00050 Nav2Remote::~Nav2Remote()
00051 {
00052     if(line) free(line);
00053     close(fd);
00054 }
00055 
00056 int Nav2Remote::readLine() const
00057 {
00058     for( int pos=0;; ++pos) {
00059         if( lineLen <= pos+1) {
00060             lineLen += 32;
00061             line = (char*)realloc(line, lineLen);
00062             if( !line) throw std::bad_alloc();
00063         }
00064         if( read(fd, &line[pos], 1) <= 0) return -1;
00065 
00066         if( line[pos] == '\r') {
00067             --pos;  // Ignore carriage returns, just in case!
00068             continue;
00069         }
00070 
00071         if( line[pos] == '\n') {
00072             line[pos] = 0;
00073 
00074             // Ignore lines that begin with | or +.
00075             if( line[0] == '|' || line[0] == '+') {
00076                 pos = -1;
00077                 continue;
00078             }
00079 
00080             return pos;
00081         }
00082     }
00083 }
00084 
00085 int Nav2Remote::setTargetOrientation( double orientation)
00086 {
00087     char msg[128];
00088     int p = sprintf(msg, "o %lf\n", orientation);
00089     return write(fd,msg,p) == p ? 0 : -1;
00090 }
00091 
00092 int Nav2Remote::setAbsoluteVelocity( double vx, double vy)
00093 {
00094     char msg[128];
00095     int p = sprintf(msg, "av %lf %lf\n", vx, vy);
00096     return write(fd,msg,p) == p ? 0 : -1;
00097 }
00098 
00099 int Nav2Remote::setRelativeVelocity( double vx, double vy, double turnRate)
00100 {
00101     char msg[128];
00102     int p = sprintf(msg, "v %lf %lf %lf\n", vx, vy, turnRate * (180.0 / M_PI));
00103     return write(fd,msg,p) == p ? 0 : -1;
00104 }
00105 
00106 int Nav2Remote::estimatePosition(
00107     double& x, double& y, double& orientation) const
00108 {
00109     if( write(fd, "q\n", 2) != 2) return -1;
00110 
00111     // Read the result
00112     if( readLine() < 0) return -1;
00113 
00114     int qlen;
00115     sscanf(line, "%lf %lf %lf %d", &x, &y, &orientation, &qlen);
00116     orientation *= (M_PI / 180.0);
00117 
00118     return 0;
00119 }
00120 
00121 int Nav2Remote::setPosition( double x, double y, double orientation)
00122 {
00123     char msg[128];
00124     int p = sprintf(msg, "p %lf %lf %lf\n", x, y, orientation * (180.0 / M_PI));
00125     return write(fd,msg,p) == p ? 0 : -1;
00126 }
00127 
00128 int Nav2Remote::stop()
00129 {
00130     return write(fd,"s\n",2) == 2 ? 0 : -1;
00131 }
00132 
00133 int Nav2Remote::turnLeft( double angle)
00134 {
00135     char msg[128];
00136     int p = sprintf(msg, "lt %lf\n", angle * (180.0 / M_PI));
00137     return write(fd,msg,p) == p ? 0 : -1;
00138 }
00139 
00140 int Nav2Remote::move( double dist, double direction)
00141 {
00142     char msg[128];
00143     int p = sprintf(msg, "mv %lf %lf\n", dist, direction * (180.0 / M_PI));
00144     return write(fd,msg,p) == p ? 0 : -1;
00145 }
00146 
00147 int Nav2Remote::setMaxSpeed( double maxSpeed)
00148 {
00149     char msg[128];
00150     int p = sprintf(msg, "sms %lf\n", maxSpeed);
00151     return write(fd,msg,p) == p ? 0 : -1;
00152 }
00153 
00154 int Nav2Remote::setMaxAccel( double maxAccel)
00155 {
00156     char msg[128];
00157     int p = sprintf(msg, "sma %lf\n", maxAccel);
00158     return write(fd,msg,p) == p ? 0 : -1;
00159 }
00160 
00161 int Nav2Remote::setMaxCorneringError( double maxCorneringError)
00162 {
00163     char msg[128];
00164     int p = sprintf(msg, "smce %lf\n", maxCorneringError);
00165     return write(fd,msg,p) == p ? 0 : -1;
00166 }
00167 
00168 double Nav2Remote::getMaxSpeed() const
00169 {
00170     if( write(fd, "qms\n", 4) != 4) return -1;
00171 
00172     // Read the result
00173     if( readLine() < 0) return -1;
00174 
00175     double result;
00176     sscanf(line, "%lf", &result);
00177 
00178     return result;
00179 }
00180 
00181 double Nav2Remote::getMaxAccel() const
00182 {
00183     if( write(fd, "qma\n", 4) != 4) return -1;
00184 
00185     // Read the result
00186     if( readLine() < 0) return -1;
00187 
00188     double result;
00189     sscanf(line, "%lf", &result);
00190 
00191     return result;
00192 }
00193 
00194 double Nav2Remote::getMaxCorneringError() const
00195 {
00196     if( write(fd, "qmce\n", 5) != 5) return -1;
00197 
00198     // Read the result
00199     if( readLine() < 0) return -1;
00200 
00201     double result;
00202     sscanf(line, "%lf", &result);
00203 
00204     return result;
00205 }
00206 
00207 int Nav2Remote::getQueueSize() const
00208 {
00209     if( write(fd, "q\n", 2) != 2) return -1;
00210 
00211     // Read the result
00212     if( readLine() < 0) return -1;
00213 
00214     double x;
00215     int qlen;
00216     sscanf(line, "%lf %lf %lf %d", &x, &x, &x, &qlen);
00217 
00218     return qlen;
00219 }
00220 
00221 int Nav2Remote::wait() const
00222 {
00223     while(1) {
00224         int rc = getQueueSize();
00225         if( rc < 1) return rc;
00226         usleep(100000);
00227     }
00228 }


nav2_driver
Author(s): Paul Bovbel
autogenerated on Fri Aug 28 2015 11:45:16