hokuyo.cpp
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2008-2010  Willow Garage
00004  *                      
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 
00022 #include <stdio.h>
00023 #include <string.h>
00024 #include <errno.h>
00025 #include <termios.h>
00026 #include <math.h>
00027 #include <poll.h>
00028 #include <signal.h>
00029 
00030 #include "hokuyo_node/hokuyo.h"
00031 
00032 #include <algorithm>
00033 
00034 #include <time.h>
00035 
00036 #include <fcntl.h>
00037 
00038 #include "ros/console.h"
00039 
00040 #if POSIX_TIMERS <= 0
00041 #include <sys/time.h>
00042 #endif
00043 
00044 
00046 #define HOKUYO_EXCEPT(except, msg, ...) \
00047   { \
00048     char buf[1000]; \
00049     snprintf(buf, 1000, msg " (in hokuyo::laser::%s) You may find further details at http://www.ros.org/wiki/hokuyo_node/Troubleshooting" , ##__VA_ARGS__, __FUNCTION__); \
00050     throw except(buf); \
00051   }
00052 
00053 
00055 static uint64_t timeHelper()
00056 {
00057 #if POSIX_TIMERS > 0
00058   struct timespec curtime;
00059   clock_gettime(CLOCK_REALTIME, &curtime);
00060   return (uint64_t)(curtime.tv_sec) * 1000000000 + (uint64_t)(curtime.tv_nsec);  
00061 #else
00062   struct timeval timeofday;
00063   gettimeofday(&timeofday,NULL);
00064   return (uint64_t)(timeofday.tv_sec) * 1000000000 + (uint64_t)(timeofday.tv_usec) * 1000;  
00065 #endif
00066 }
00067 
00068 
00069 #ifdef USE_LOG_FILE
00070 FILE *logfile;
00071 #endif
00072 
00074 hokuyo::Laser::Laser() :
00075                       dmin_(0), dmax_(0), ares_(0), amin_(0), amax_(0), afrt_(0), rate_(0),
00076                       wrapped_(0), last_time_(0), time_repeat_count_(0), offset_(0),
00077                       laser_fd_(-1)
00078 { 
00079 #ifdef USE_LOG_FILE
00080   if (!logfile)
00081     logfile = fopen("hokuyo.log", "w");
00082 #endif
00083 }
00084 
00085 
00087 hokuyo::Laser::~Laser ()
00088 {
00089   if (portOpen())
00090     close();
00091 }
00092 
00093 
00095 void
00096 hokuyo::Laser::open(const char * port_name)
00097 {
00098   if (portOpen())
00099     close();
00100   
00101   // Make IO non blocking. This way there are no race conditions that
00102   // cause blocking when a badly behaving process does a read at the same
00103   // time as us. Will need to switch to blocking for writes or errors
00104   // occur just after a replug event.
00105   laser_fd_ = ::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY);
00106   read_buf_start = read_buf_end = 0;
00107 
00108   if (laser_fd_ == -1)
00109   {
00110     const char *extra_msg = "";
00111     switch (errno)
00112     {
00113       case EACCES:
00114         extra_msg = "You probably don't have premission to open the port for reading and writing.";
00115         break;
00116       case ENOENT:
00117         extra_msg = "The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?";
00118         break;
00119     }
00120 
00121     HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg);
00122   }
00123   try
00124   {
00125     struct flock fl;
00126     fl.l_type   = F_WRLCK;
00127     fl.l_whence = SEEK_SET;
00128     fl.l_start = 0;
00129     fl.l_len   = 0;
00130     fl.l_pid   = getpid();
00131 
00132     if (fcntl(laser_fd_, F_SETLK, &fl) != 0)
00133       HOKUYO_EXCEPT(hokuyo::Exception, "Device %s is already locked. Try 'lsof | grep %s' to find other processes that currently have the port open.", port_name, port_name);
00134 
00135     // Settings for USB?
00136     struct termios newtio;
00137     tcgetattr(laser_fd_, &newtio);
00138     memset (&newtio.c_cc, 0, sizeof (newtio.c_cc));
00139     newtio.c_cflag = CS8 | CLOCAL | CREAD;
00140     newtio.c_iflag = IGNPAR;
00141     newtio.c_oflag = 0;
00142     newtio.c_lflag = 0;
00143 
00144     // activate new settings
00145     tcflush (laser_fd_, TCIFLUSH);
00146     if (tcsetattr (laser_fd_, TCSANOW, &newtio) < 0)
00147       HOKUYO_EXCEPT(hokuyo::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); 
00148     usleep (200000);
00149 
00150     // Some models (04LX) need to be told to go into SCIP2 mode...
00151     laserFlush();
00152     // Just in case a previous failure mode has left our Hokuyo
00153     // spewing data, we send reset the laser to be safe.
00154     try {
00155       reset();
00156     }
00157     catch (hokuyo::Exception &e)
00158     { 
00159       // This might be a device that needs to be explicitely placed in
00160       // SCIP2 mode. 
00161       // Note: Not tested: a device that is currently scanning in SCIP1.1
00162       // mode might not manage to switch to SCIP2.0.
00163       
00164       setToSCIP2(); // If this fails then it wasn't a device that could be switched to SCIP2.
00165       reset(); // If this one fails, it really is an error.
00166     }
00167 
00168     querySensorConfig();
00169 
00170     queryVersionInformation(); // In preparation for calls to get various parts of the version info.
00171   }
00172   catch (hokuyo::Exception& e)
00173   {
00174     // These exceptions mean something failed on open and we should close
00175     if (laser_fd_ != -1)
00176       ::close(laser_fd_);
00177     laser_fd_ = -1;
00178     throw e;
00179   }
00180 }
00181 
00182 
00184 void hokuyo::Laser::reset ()
00185 {
00186         if (!portOpen())
00187             HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00188         laserFlush();
00189         try
00190         {
00191           sendCmd("TM2", 1000);
00192         }
00193         catch (hokuyo::Exception &e)
00194         {} // Ignore. If the laser was scanning TM2 would fail
00195         try
00196         {
00197           sendCmd("RS", 1000);
00198           last_time_ = 0; // RS resets the hokuyo clock.
00199           wrapped_ = 0; // RS resets the hokuyo clock.
00200         }
00201         catch (hokuyo::Exception &e)
00202         {} // Ignore. If the command coincided with a scan we might get garbage.
00203         laserFlush();
00204         sendCmd("RS", 1000); // This one should just work.
00205 }
00206 
00207 
00209 void
00210 hokuyo::Laser::close ()
00211 {
00212   int retval = 0;
00213 
00214   if (portOpen()) {
00215     //Try to be a good citizen and completely shut down the laser before we shutdown communication
00216     try
00217     {
00218       reset();
00219     }
00220     catch (hokuyo::Exception& e) {
00221       //Exceptions here can be safely ignored since we are closing the port anyways
00222     }
00223 
00224     retval = ::close(laser_fd_); // Automatically releases the lock.
00225   }
00226 
00227   laser_fd_ = -1;
00228 
00229   if (retval != 0)
00230     HOKUYO_EXCEPT(hokuyo::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno));
00231 }
00232 
00234 void
00235 hokuyo::Laser::setToSCIP2()
00236 {
00237         if (!portOpen())
00238             HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00239         const char * cmd = "SCIP2.0";
00240         char buf[100];
00241         laserWrite(cmd);
00242         laserWrite("\n");
00243 
00244         laserReadline(buf, 100, 1000);
00245         ROS_DEBUG("Laser comm protocol changed to %s \n", buf);
00246         //printf ("Laser comm protocol changed to %s \n", buf);
00247 }
00248 
00249 
00251 int
00252 hokuyo::Laser::sendCmd(const char* cmd, int timeout)
00253 {
00254 if (!portOpen())
00255     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00256 
00257   char buf[100]; 
00258 
00259   laserWrite(cmd);
00260   laserWrite("\n");
00261 
00262   laserReadlineAfter(buf, 100, cmd, timeout);
00263   laserReadline(buf,100,timeout);
00264 
00265   if (!checkSum(buf,4))
00266     HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code.");
00267 
00268   buf[2] = 0;
00269   
00270   if (buf[0] - '0' >= 0 && buf[0] - '0' <= 9 && buf[1] - '0' >= 0 && buf[1] - '0' <= 9)
00271     return (buf[0] - '0')*10 + (buf[1] - '0');
00272   else
00273     HOKUYO_EXCEPT(hokuyo::Exception, "Hokuyo error code returned. Cmd: %s --  Error: %s", cmd, buf);
00274 }
00275 
00276 
00278 void
00279 hokuyo::Laser::getConfig(LaserConfig& config)
00280 {
00281   config.min_angle  =  (amin_ - afrt_) * (2.0*M_PI)/(ares_);
00282   config.max_angle  =  (amax_ - afrt_) * (2.0*M_PI)/(ares_);
00283   config.ang_increment =  (2.0*M_PI)/(ares_);
00284   config.time_increment = (60.0)/(double)(rate_ * ares_);
00285   config.scan_time = 60.0/((double)(rate_));
00286   config.min_range  =  dmin_ / 1000.0;
00287   config.max_range  =  dmax_ / 1000.0;
00288 }
00289 
00290 
00292 int
00293 hokuyo::Laser::laserWrite(const char* msg)
00294 {
00295   // IO is currently non-blocking. This is what we want for the more common read case.
00296   int origflags = fcntl(laser_fd_,F_GETFL,0);
00297   fcntl(laser_fd_, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking?
00298   ssize_t len = strlen(msg);
00299   ssize_t retval = write(laser_fd_, msg, len);
00300   int fputserrno = errno;
00301   fcntl(laser_fd_, F_SETFL, origflags | O_NONBLOCK);
00302   errno = fputserrno; // Don't want to see the fcntl errno below.
00303   
00304   if (retval != -1)
00305   {
00306 #ifdef USE_LOG_FILE
00307     if (strlen(msg) > 1)
00308     {
00309       long long outtime = timeHelper();
00310       fprintf(logfile, "Out: %lli.%09lli %s\n", outtime / 1000000000L, outtime % 1000000000L, msg);
00311     }
00312 #endif
00313     return retval;
00314   }
00315   else
00316     HOKUYO_EXCEPT(hokuyo::Exception, "fputs failed -- Error = %d: %s", errno, strerror(errno));
00317 }
00318 
00319 
00321 int
00322 hokuyo::Laser::laserFlush()
00323 {
00324   int retval = tcflush(laser_fd_, TCIOFLUSH);
00325   if (retval != 0)
00326     HOKUYO_EXCEPT(hokuyo::Exception, "tcflush failed");
00327   read_buf_start = 0;
00328   read_buf_end = 0;
00329   
00330   return retval;
00331 } 
00332 
00333 
00335 int 
00336 hokuyo::Laser::laserReadline(char *buf, int len, int timeout)
00337 {
00338   int current=0;
00339 
00340   struct pollfd ufd[1];
00341   int retval;
00342   ufd[0].fd = laser_fd_;
00343   ufd[0].events = POLLIN;
00344     
00345   if (timeout == 0)
00346     timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00347 
00348   while (true)
00349   {
00350     if (read_buf_start == read_buf_end) // Need to read?
00351     {
00352       if ((retval = poll(ufd, 1, timeout)) < 0)
00353         HOKUYO_EXCEPT(hokuyo::Exception, "poll failed   --  error = %d: %s", errno, strerror(errno));
00354 
00355       if (retval == 0)
00356         HOKUYO_EXCEPT(hokuyo::TimeoutException, "timeout reached");
00357       
00358       if (ufd[0].revents & POLLERR)
00359         HOKUYO_EXCEPT(hokuyo::Exception, "error on socket, possibly unplugged");
00360       
00361       int bytes = read(laser_fd_, read_buf, sizeof(read_buf));
00362       if (bytes == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
00363         HOKUYO_EXCEPT(hokuyo::Exception, "read failed");
00364       read_buf_start = 0;
00365       read_buf_end = bytes;
00366     }
00367        
00368     while (read_buf_end != read_buf_start)
00369     {
00370       if (current == len - 1)
00371       {
00372         buf[current] = 0;
00373         HOKUYO_EXCEPT(hokuyo::Exception, "buffer filled without end of line being found");
00374       }
00375 
00376       buf[current] = read_buf[read_buf_start++];
00377       if (buf[current++] == '\n')
00378       {
00379         buf[current] = 0;
00380         return current;
00381       }
00382     }
00383 
00384 #ifdef USE_LOG_FILE
00385     long long outtime = timeHelper();
00386     fprintf(logfile, "In: %lli.%09lli %s", outtime / 1000000000L, outtime % 1000000000L, buf);
00387 #endif
00388   }
00389 }
00390 
00391 
00392 char*
00393 hokuyo::Laser::laserReadlineAfter(char* buf, int len, const char* str, int timeout)
00394 {
00395   buf[0] = 0;
00396   char* ind = &buf[0];
00397 
00398   int bytes_read = 0;
00399   int skipped = 0;
00400 
00401   while ((strncmp(buf, str, strlen(str))) != 0) {
00402     bytes_read = laserReadline(buf,len,timeout);
00403 
00404     if ((skipped += bytes_read) > MAX_SKIPPED)
00405       HOKUYO_EXCEPT(hokuyo::Exception, "too many bytes skipped while searching for match");
00406   }
00407 
00408   return ind += strlen(str);
00409 }
00410 
00411 
00413 void
00414 hokuyo::Laser::querySensorConfig()
00415 {
00416   if (!portOpen())
00417     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00418 
00419   if (sendCmd("PP",1000) != 0)
00420     HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting configuration information");
00421 
00422   char buf[100];
00423   char* ind;
00424     
00425   ind = laserReadlineAfter(buf,100,"DMIN:",-1);
00426   sscanf(ind, "%d", &dmin_);
00427     
00428   ind = laserReadlineAfter(buf,100,"DMAX:",-1);
00429   sscanf(ind, "%d", &dmax_);
00430     
00431   ind = laserReadlineAfter(buf,100,"ARES:",-1);
00432   sscanf(ind, "%d", &ares_);
00433     
00434   ind = laserReadlineAfter(buf,100,"AMIN:",-1);
00435   sscanf(ind, "%d", &amin_);
00436     
00437   ind = laserReadlineAfter(buf,100,"AMAX:",-1);
00438   sscanf(ind, "%d", &amax_);
00439     
00440   ind = laserReadlineAfter(buf,100,"AFRT:",-1);
00441   sscanf(ind, "%d", &afrt_);
00442     
00443   ind = laserReadlineAfter(buf,100,"SCAN:",-1);
00444   sscanf(ind, "%d", &rate_);
00445     
00446   return;
00447 }
00448 
00449 
00451 bool
00452 hokuyo::Laser::checkSum(const char* buf, int buf_len)
00453 {
00454   char sum = 0;
00455   for (int i = 0; i < buf_len - 2; i++)
00456     sum += (unsigned char)(buf[i]);
00457 
00458   if ((sum & 63) + 0x30 == buf[buf_len - 2])
00459     return true;
00460   else
00461     return false;
00462 }
00463 
00464 
00466 uint64_t
00467 hokuyo::Laser::readTime(int timeout)
00468 {
00469   char buf[100];
00470 
00471   laserReadline(buf, 100, timeout);
00472   if (!checkSum(buf, 6))
00473     HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on time stamp.");
00474 
00475   unsigned int laser_time = ((buf[0]-0x30) << 18) | ((buf[1]-0x30) << 12) | ((buf[2]-0x30) << 6) | (buf[3] - 0x30);
00476 
00477   if (laser_time == last_time_)
00478   {
00479     if (++time_repeat_count_ > 2)
00480     {
00481       HOKUYO_EXCEPT(hokuyo::RepeatedTimeException, "The timestamp has not changed for %d reads", time_repeat_count_);
00482     }
00483     else if (time_repeat_count_ > 0)
00484       ROS_DEBUG("The timestamp has not changed for %d reads. Ignoring for now.", time_repeat_count_);
00485   }
00486   else
00487   {
00488     time_repeat_count_ = 0;
00489   }
00490 
00491   if (laser_time < last_time_)
00492     wrapped_++;
00493   
00494   last_time_ = laser_time;
00495   
00496   return (uint64_t)((wrapped_ << 24) | laser_time)*(uint64_t)(1000000);
00497 }
00498 
00499 
00501 void
00502 hokuyo::Laser::readData(hokuyo::LaserScan& scan, bool has_intensity, int timeout)
00503 {
00504   scan.ranges.clear();
00505   scan.intensities.clear();
00506 
00507   int data_size = 3;
00508   if (has_intensity)
00509     data_size = 6;
00510 
00511   char buf[100];
00512 
00513   int ind = 0;
00514 
00515   scan.self_time_stamp = readTime(timeout);
00516 
00517   int bytes;
00518 
00519   int range;
00520   float intensity;
00521 
00522   for (;;)
00523   {
00524     bytes = laserReadline(&buf[ind], 100 - ind, timeout);
00525     
00526     if (bytes == 1)          // This is \n\n so we should be done
00527       return;
00528     
00529     if (!checkSum(&buf[ind], bytes))
00530       HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read.");
00531     
00532     bytes += ind - 2;
00533     
00534     // Read as many ranges as we can get
00535     if(dmax_ > 20000){ // Check error codes for the UTM 30LX (it is the only one with the long max range and has different error codes)
00536       for (int j = 0; j < bytes - (bytes % data_size); j+=data_size)
00537       {
00538         if (scan.ranges.size() < MAX_READINGS)
00539         {
00540           range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30));
00541           
00542           switch (range) // See the SCIP2.0 reference on page 12, Table 4
00543           {
00544             case 1: // No Object in Range
00545               scan.ranges.push_back(std::numeric_limits<float>::infinity());
00546               break;
00547             case 2: // Object is too near (Internal Error)
00548               scan.ranges.push_back(-std::numeric_limits<float>::infinity());
00549               break;
00550             case 3: // Measurement Error (May be due to interference)
00551               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00552               break;
00553             case 4: // Object out of range (at the near end)
00555               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00556               break;
00557             case 5: // Other errors
00558               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00559               break;
00560             default:
00561               scan.ranges.push_back(((float)range)/1000.0);
00562           }
00563 
00564           if (has_intensity)
00565           {
00566             intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30));
00567             scan.intensities.push_back(intensity);
00568           }
00569         }
00570         else
00571         {
00572           HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected");
00573         }
00574       }
00575     } else { // Check error codes for all other lasers (URG-04LX UBG-04LX-F01 UHG-08LX)
00576       for (int j = 0; j < bytes - (bytes % data_size); j+=data_size)
00577       {
00578         if (scan.ranges.size() < MAX_READINGS)
00579         {
00580           range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30));
00581           
00582           switch (range) // See the SCIP2.0 reference on page 12, Table 3
00583           {
00584             case 0: // Detected object is possibly at 22m
00585               scan.ranges.push_back(std::numeric_limits<float>::infinity());
00586               break;
00587             case 1: // Reflected light has low intensity
00588               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00589               break;
00590             case 2: // Reflected light has low intensity
00591               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00592               break;
00593             case 6: // Detected object is possibly at 5.7m
00594               scan.ranges.push_back(std::numeric_limits<float>::infinity());
00595               break;
00596             case 7: // Distance data on the preceding and succeeding steps have errors
00597               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00598               break;
00599             case 8: // Intensity difference of two waves
00600               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00601               break;
00602             case 9: // The same step had error in the last two scan
00603               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00604               break;
00605             case 10: // Others
00606               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00607               break;
00608             case 11: // Others
00609               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00610               break;
00611             case 12: // Others
00612               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00613               break;
00614             case 13: // Others
00615               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00616               break;
00617             case 14: // Others
00618               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00619               break;
00620             case 15: // Others
00621               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00622               break;
00623             case 16: // Others
00624               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00625               break;
00626             case 17: // Others
00627               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00628               break;
00629             case 18: // Error reading due to strong reflective object
00630               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00631               break;
00632             case 19: // Non-Measurable step
00633               scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00634               break;
00635             default:
00636               scan.ranges.push_back(((float)range)/1000.0);
00637           }
00638 
00639           if (has_intensity)
00640           {
00641             intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30));
00642             scan.intensities.push_back(intensity);
00643           }
00644         }
00645         else
00646         {
00647           HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected");
00648         }
00649       }
00650     }
00651     // Shuffle remaining bytes to front of buffer to get them on the next loop
00652     ind = 0;
00653     for (int j = bytes - (bytes % data_size); j < bytes ; j++)
00654       buf[ind++] = buf[j];
00655   }
00656 }
00657 
00658 
00660 int
00661 hokuyo::Laser::pollScan(hokuyo::LaserScan& scan, double min_ang, double max_ang, int cluster, int timeout)
00662 {
00663   if (!portOpen())
00664     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00665 
00666   int status;
00667 
00668   // Always clear ranges/intensities so we can return easily in case of erro
00669   scan.ranges.clear();
00670   scan.intensities.clear();
00671 
00672   // clustering of 0 and 1 are actually the same
00673   if (cluster == 0)
00674     cluster = 1;
00675   
00676   int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI));
00677   int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI));
00678   
00679   char cmdbuf[MAX_CMD_LEN];
00680   
00681   sprintf(cmdbuf,"GD%.4d%.4d%.2d", min_i, max_i, cluster);
00682   
00683   status = sendCmd(cmdbuf, timeout);
00684   
00685   scan.system_time_stamp = timeHelper() + offset_;
00686   
00687   if (status != 0)
00688     return status;
00689   
00690   // Populate configuration
00691   scan.config.min_angle  =  (min_i - afrt_) * (2.0*M_PI)/(ares_);
00692   scan.config.max_angle  =  (max_i - afrt_) * (2.0*M_PI)/(ares_);
00693   scan.config.ang_increment =  cluster*(2.0*M_PI)/(ares_);
00694   scan.config.time_increment = (60.0)/(double)(rate_ * ares_);
00695   scan.config.scan_time = 0.0;
00696   scan.config.min_range  =  dmin_ / 1000.0;
00697   scan.config.max_range  =  dmax_ / 1000.0;
00698   
00699   readData(scan, false, timeout);
00700   
00701   long long inc = (long long)(min_i * scan.config.time_increment * 1000000000);
00702   
00703   scan.system_time_stamp += inc;
00704   scan.self_time_stamp += inc;
00705   
00706   return 0;
00707 }
00708 
00709 int
00710 hokuyo::Laser::laserOn() {
00711   int res = sendCmd("BM",1000);
00712   if (res == 1)
00713     HOKUYO_EXCEPT(hokuyo::Exception, "Unable to control laser due to malfunction.");
00714   return res;
00715 }
00716 
00717 int
00718 hokuyo::Laser::laserOff() {
00719   return sendCmd("QT",1000);
00720 }
00721 
00722 int
00723 hokuyo::Laser::stopScanning() {
00724   try {
00725     return laserOff();
00726   }
00727   catch (hokuyo::Exception &e)
00728   {
00729     // Ignore exception because we might have gotten part of a scan 
00730     // instead of the expected response, which shows up as a bad checksum.
00731     laserFlush();
00732   } 
00733   return laserOff(); // This one should work because the scan is stopped.
00734 }
00735 
00737 int
00738 hokuyo::Laser::requestScans(bool intensity, double min_ang, double max_ang, int cluster, int skip, int count, int timeout)
00739 {
00740   if (!portOpen())
00741     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00742 
00744 
00745   int status;
00746 
00747   if (cluster == 0)
00748     cluster = 1;
00749   
00750   int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI));
00751   int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI));
00752   
00753   char cmdbuf[MAX_CMD_LEN];
00754   
00755   char intensity_char = 'D';
00756   if (intensity)
00757     intensity_char = 'E';
00758   
00759   sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count);
00760   
00761   status = sendCmd(cmdbuf, timeout);
00762   
00763   return status;
00764 }
00765 
00766 bool
00767 hokuyo::Laser::isIntensitySupported()
00768 {
00769   hokuyo::LaserScan  scan;
00770   
00771   if (!portOpen())
00772     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00773 
00774   // Try an intensity command.
00775   try
00776   {
00777     requestScans(1, 0, 0, 0, 0, 1);
00778     serviceScan(scan, 1000);
00779     return true;
00780   }
00781   catch (hokuyo::Exception &e)
00782   {} 
00783   
00784   // Try a non intensity command.
00785   try
00786   {
00787     requestScans(0, 0, 0, 0, 0, 1);
00788     serviceScan(scan, 1000);
00789     return false;
00790   }
00791   catch (hokuyo::Exception &e)
00792   {
00793     HOKUYO_EXCEPT(hokuyo::Exception, "Exception whil trying to determine if intensity scans are supported.")
00794   } 
00795 }
00796 
00797 int
00798 hokuyo::Laser::serviceScan(hokuyo::LaserScan& scan, int timeout)
00799 {
00800   if (!portOpen())
00801     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00802 
00803   // Always clear ranges/intensities so we can return easily in case of erro
00804   scan.ranges.clear();
00805   scan.intensities.clear();
00806 
00807   char buf[100];
00808 
00809   bool intensity = false;
00810   int min_i;
00811   int max_i;
00812   int cluster;
00813   int skip;
00814   int left;
00815 
00816   char* ind;
00817 
00818   int status = -1;
00819 
00820   do {
00821     ind = laserReadlineAfter(buf, 100, "M",timeout);
00822     scan.system_time_stamp = timeHelper() + offset_;
00823 
00824     if (ind[0] == 'D')
00825       intensity = false;
00826     else if (ind[0] == 'E')
00827       intensity = true;
00828     else
00829       continue;
00830 
00831     ind++;
00832 
00833     sscanf(ind, "%4d%4d%2d%1d%2d", &min_i, &max_i, &cluster, &skip, &left);  
00834     laserReadline(buf,100,timeout);
00835 
00836     buf[4] = 0;
00837 
00838     if (!checkSum(buf, 4))
00839       HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code: %s", buf);
00840 
00841     sscanf(buf, "%2d", &status);
00842 
00843     if (status != 99)
00844       return status;
00845     
00846   } while(status != 99);
00847     
00848   scan.config.min_angle  =  (min_i - afrt_) * (2.0*M_PI)/(ares_);
00849   scan.config.max_angle  =  (max_i - afrt_) * (2.0*M_PI)/(ares_);
00850   scan.config.ang_increment =  cluster*(2.0*M_PI)/(ares_);
00851   scan.config.time_increment = (60.0)/(double)(rate_ * ares_);
00852   scan.config.scan_time = (60.0 * (skip + 1))/((double)(rate_));
00853   scan.config.min_range  =  dmin_ / 1000.0;
00854   scan.config.max_range  =  dmax_ / 1000.0;
00855 
00856   readData(scan, intensity, timeout);
00857 
00858   long long inc = (long long)(min_i * scan.config.time_increment * 1000000000);
00859 
00860   scan.system_time_stamp += inc;
00861   scan.self_time_stamp += inc;
00862 
00863 //  printf("Scan took %lli.\n", -scan.system_time_stamp + timeHelper() + offset_);
00864 
00865   return 0;
00866 }
00867 
00869 void
00870 hokuyo::Laser::queryVersionInformation()
00871 {
00872   if (!portOpen())
00873     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00874 
00875   if (sendCmd("VV",1000) != 0)
00876     HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting version information");
00877   
00878   char buf[100];
00879   vendor_name_ = laserReadlineAfter(buf, 100, "VEND:");
00880   vendor_name_ = vendor_name_.substr(0,vendor_name_.length() - 3);
00881   
00882   product_name_ = laserReadlineAfter(buf, 100, "PROD:");
00883   product_name_ = product_name_.substr(0,product_name_.length() - 3);
00884   
00885   firmware_version_ = laserReadlineAfter(buf, 100, "FIRM:");
00886   firmware_version_ = firmware_version_.substr(0,firmware_version_.length() - 3);
00887 
00888   protocol_version_ = laserReadlineAfter(buf, 100, "PROT:");
00889   protocol_version_ = protocol_version_.substr(0,protocol_version_.length() - 3);
00890   
00891   // This crazy naming scheme is for backward compatibility. Initially
00892   // the serial number always started with an H. Then it got changed to a
00893   // zero. For a while the driver was removing the leading zero in the
00894   // serial number. This is fine as long as it is indeed a zero in front.
00895   // The current behavior is backward compatible but will accomodate full
00896   // length serial numbers.
00897   serial_number_ = laserReadlineAfter(buf, 100, "SERI:");
00898   serial_number_ = serial_number_.substr(0,serial_number_.length() - 3);
00899   if (serial_number_[0] == '0')
00900     serial_number_[0] = 'H';
00901   else if (serial_number_[0] != 'H')
00902     serial_number_ = 'H' + serial_number_;
00903 }
00904 
00905 
00907 std::string 
00908 hokuyo::Laser::getID()
00909 {
00910   if (!portOpen())
00911     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00912 
00913   return serial_number_;
00914 }
00915 
00916 
00918 std::string 
00919 hokuyo::Laser::getFirmwareVersion()
00920 {
00921   if (!portOpen())
00922     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00923 
00924   return firmware_version_;
00925 }
00926 
00927 
00929 std::string 
00930 hokuyo::Laser::getProtocolVersion()
00931 {
00932   if (!portOpen())
00933     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00934 
00935   return protocol_version_;
00936 }
00937 
00938 
00940 std::string 
00941 hokuyo::Laser::getVendorName()
00942 {
00943   if (!portOpen())
00944     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00945 
00946   return vendor_name_;
00947 }
00948 
00949 
00951 std::string 
00952 hokuyo::Laser::getProductName()
00953 {
00954   if (!portOpen())
00955     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00956 
00957   return product_name_;
00958 }
00959 
00960 
00962 std::string
00963 hokuyo::Laser::getStatus()
00964 {
00965   if (!portOpen())
00966     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
00967 
00968   if (sendCmd("II",1000) != 0)
00969     HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting device information information");
00970   
00971   char buf[100];
00972   char* stat = laserReadlineAfter(buf, 100, "STAT:");
00973 
00974   std::string statstr(stat);
00975   statstr = statstr.substr(0,statstr.length() - 3);
00976 
00977   return statstr;
00978 }
00979     
00980 template <class C>
00981 C median(std::vector<C> &v)
00982 {
00983   std::vector<long long int>::iterator start  = v.begin();
00984   std::vector<long long int>::iterator end    = v.end();
00985   std::vector<long long int>::iterator median = start + (end - start) / 2;
00986   //std::vector<long long int>::iterator quarter1 = median - (end - start) / 4;
00987   //std::vector<long long int>::iterator quarter2 = median + (end - start) / 4;
00988   std::nth_element(start, median, end);
00989   //long long int medianval = *median;
00990   //std::nth_element(start, quarter1, end);
00991   //long long int quarter1val = *quarter1;
00992   //std::nth_element(start, quarter2, end);
00993   //long long int quarter2val = *quarter2;
00994   return *median;
00995 }
00996 
00997 #if 1
00998 long long int hokuyo::Laser::getHokuyoClockOffset(int reps, int timeout)
00999 {
01000   std::vector<long long int> offset(reps);
01001 
01002   sendCmd("TM0",timeout);
01003   for (int i = 0; i < reps; i++)
01004   {
01005     long long int prestamp = timeHelper();
01006     sendCmd("TM1",timeout);
01007     long long int hokuyostamp = readTime();
01008     long long int poststamp = timeHelper();
01009     offset[i] = hokuyostamp - (prestamp + poststamp) / 2;
01010     //printf("%lli %lli %lli", hokuyostamp, prestamp, poststamp);
01011   }
01012   sendCmd("TM2",timeout);
01013   
01014   long long out = median(offset);
01015 
01016   return out;
01017 }
01018 
01019 long long int hokuyo::Laser::getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout)
01020 {
01021   if (reps < 1)
01022     reps = 1;
01023   else if (reps > 99) 
01024     reps = 99;
01025   
01026   std::vector<long long int> offset(reps);
01027   
01028   if (requestScans(intensity, min_ang, max_ang, clustering, skip, reps, timeout) != 0)
01029   {
01030     HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scan while caliblating time.");
01031     return 1;
01032   }
01033 
01034   hokuyo::LaserScan scan;
01035   for (int i = 0; i < reps; i++)
01036   {
01037     serviceScan(scan, timeout);
01038     //printf("%lli %lli\n", scan.self_time_stamp, scan.system_time_stamp);
01039     offset[i] = scan.self_time_stamp - scan.system_time_stamp;
01040   }
01041 
01042   return median(offset);
01043 }
01044 
01046 long long
01047 hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
01048 {
01049   offset_ = 0;
01050   if (!portOpen())
01051     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
01052 
01053   if (num <= 0)
01054     num = 10;
01055   
01056   int ckreps = 1;
01057   int scanreps = 1;
01058   long long int start = getHokuyoClockOffset(ckreps, timeout);
01059   long long int pre = 0;
01060   std::vector<long long int> samples(num);
01061   for (int i = 0; i < num; i++)
01062   {                                                 
01063     long long int scan = getHokuyoScanStampToSystemStampOffset(intensity, min_ang, max_ang, clustering, skip, scanreps, timeout) - start; 
01064     long long int post = getHokuyoClockOffset(ckreps, timeout) - start;
01065     samples[i] = scan - (post+pre)/2;
01066     //printf("%lli %lli %lli %lli %lli\n", samples[i], post, pre, scan, pre - post);
01067     //fflush(stdout);
01068     pre = post;
01069   }
01070 
01071   offset_ = median(samples);
01072   //printf("%lli\n", median(samples));
01073   return offset_;
01074 }
01075 
01076 #else
01077 
01078 long long
01079 hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
01080 {
01081   ROS_DEBUG("Entering calcLatency.");
01082 
01083   if (!portOpen())
01084     HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
01085 
01086   static const std::string buggy_version = "1.16.02(19/Jan./2010)";
01087   if (firmware_version_ == buggy_version) 
01088   {
01089     ROS_INFO("Hokuyo firmware version %s detected. Using hard-coded time offset of -23 ms.", 
01090         buggy_version.c_str());
01091     offset_ = -23000000;
01092   }
01093   else
01094   {
01095     offset_ = 0;
01096 
01097     uint64_t comp_time = 0;
01098     uint64_t laser_time = 0;
01099     long long diff_time = 0;
01100     long long drift_time = 0;
01101     long long tmp_offset1 = 0;
01102     long long tmp_offset2 = 0;
01103 
01104     int count = 0;
01105 
01106     sendCmd("TM0",timeout);
01107     count = 100;
01108 
01109     for (int i = 0; i < count;i++)
01110     {
01111       usleep(1000);
01112       sendCmd("TM1",timeout);
01113       comp_time = timeHelper();
01114       try 
01115       {
01116         laser_time = readTime();
01117 
01118         diff_time = comp_time - laser_time;
01119 
01120         tmp_offset1 += diff_time / count;
01121       } catch (hokuyo::RepeatedTimeException &e)
01122       {
01123         // We expect to get Repeated Time's when hammering on the time server
01124         continue;
01125       }
01126     }
01127 
01128     uint64_t start_time = timeHelper();
01129     usleep(5000000);
01130     sendCmd("TM1;a",timeout);
01131     sendCmd("TM1;b",timeout);
01132     comp_time = timeHelper();
01133     drift_time = comp_time - start_time;
01134     laser_time = readTime() + tmp_offset1;
01135     diff_time = comp_time - laser_time;
01136     double drift_rate = double(diff_time) / double(drift_time);
01137 
01138     sendCmd("TM2",timeout);
01139 
01140     if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0)
01141       HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation");
01142 
01143     hokuyo::LaserScan scan;
01144 
01145     count = 200; 
01146     for (int i = 0; i < count;i++)
01147     {
01148       try
01149       {
01150         serviceScan(scan, 1000);
01151       } catch (hokuyo::CorruptedDataException &e) {
01152         continue;
01153       }
01154 
01155       comp_time = scan.system_time_stamp;
01156       drift_time = comp_time - start_time;
01157       laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate);
01158       diff_time = laser_time - comp_time;
01159 
01160       tmp_offset2 += diff_time / count;
01161     }
01162     
01163     offset_ = tmp_offset2;
01164 
01165     stopScanning();
01166   }
01167 
01168   ROS_DEBUG("Leaving calcLatency.");
01169 
01170   return offset_;
01171 } 
01172 #endif


hokuyo_node
Author(s): Brian P. Gerkey, Jeremy Leibs, Blaise Gassend
autogenerated on Wed Aug 26 2015 11:49:56