$search
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2008-2010 Willow Garage 00004 * 00005 * This library is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU Lesser General Public 00007 * License as published by the Free Software Foundation; either 00008 * version 2.1 of the License, or (at your option) any later version. 00009 * 00010 * This library is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00013 * Lesser General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU Lesser General Public 00016 * License along with this library; if not, write to the Free Software 00017 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00018 * 00019 */ 00020 00021 /* \mainpage hokuyo_node 00022 * \htmlinclude manifest.html 00023 */ 00024 00025 #ifndef HOKUYO_HH 00026 #define HOKUYO_HH 00027 00028 #include <stdexcept> 00029 #include <termios.h> 00030 #include <string> 00031 #include <vector> 00032 #include <stdint.h> 00033 #include <limits> 00034 00036 namespace hokuyo 00037 { 00039 const uint32_t MAX_READINGS = 1128; 00040 00042 const int MAX_CMD_LEN = 100; 00043 00045 const int MAX_SKIPPED = 1000000; 00046 00048 #define DEF_EXCEPTION(name, parent) \ 00049 class name : public parent { \ 00050 public: \ 00051 name(const char* msg) : parent(msg) {} \ 00052 } 00053 00055 DEF_EXCEPTION(Exception, std::runtime_error); 00056 00058 DEF_EXCEPTION(TimeoutException, Exception); 00059 00061 DEF_EXCEPTION(CorruptedDataException, Exception); 00062 00064 DEF_EXCEPTION(RepeatedTimeException, Exception); 00065 00066 #undef DEF_EXCEPTION 00067 00069 struct LaserConfig 00070 { 00072 float min_angle; 00074 float max_angle; 00076 float ang_increment; 00078 float time_increment; 00080 float scan_time; 00082 float min_range; 00084 float max_range; 00086 float range_res; 00087 }; 00088 00089 00091 struct LaserScan 00092 { 00094 std::vector<float> ranges; 00096 std::vector<float> intensities; 00098 uint64_t self_time_stamp; 00100 uint64_t system_time_stamp; 00102 LaserConfig config; 00103 }; 00104 00105 00107 00123 class Laser 00124 { 00125 public: 00127 Laser(); 00128 00130 ~Laser(); 00131 00133 00140 void open(const char * port_name); 00141 00143 00146 void close(); 00147 00149 00154 void reset(); 00155 00157 bool portOpen() { return laser_fd_ != -1; } 00158 00160 // sets up model 04LX to work in SCIP 2.0 mode 00161 void setToSCIP2(); 00162 00174 int sendCmd(const char* cmd, int timeout = -1); 00175 00177 00186 void getConfig(LaserConfig& config); 00187 00188 00190 00199 int pollScan(LaserScan& scan, double min_ang, double max_ang, int clustering = 0, int timeout = -1); 00200 00202 00218 int requestScans(bool intensity, double min_ang, double max_ang, int clustering = 0, int skip = 0, int num = 0, int timeout = -1); 00219 00221 00227 int serviceScan(LaserScan& scan, int timeout = -1); 00228 00230 00233 int laserOff(); 00234 00236 00239 int laserOn(); 00240 00242 00245 int stopScanning(); 00246 00248 00251 std::string getID(); 00252 00254 00257 std::string getStatus(); 00258 00260 00283 long long calcLatency(bool intensity, double min_ang, double max_ang, int clustering = 0, int skip = 0, int num = 0, int timeout = -1); 00284 00286 void clearLatency() 00287 { 00288 offset_ = 0; 00289 } 00290 00293 long long getLatency() 00294 { 00295 return offset_; 00296 } 00297 00299 00302 std::string getFirmwareVersion(); 00303 00305 00308 std::string getVendorName(); 00309 00311 00314 std::string getProductName(); 00315 00317 00320 std::string getProtocolVersion(); 00321 00323 00328 bool isIntensitySupported(); 00329 00332 void queryVersionInformation(); 00333 00335 int laserWrite(const char* msg); 00336 00338 int laserReadline(char *buf, int len, int timeout = -1); 00339 00341 int laserFlush(); 00342 00344 uint64_t readTime(int timeout = -1); 00345 00346 private: 00348 //the Hokuyo clock. Returns the median of reps measurements. 00349 long long int getHokuyoClockOffset(int reps, int timeout = -1); 00350 00352 // timestamp (PC) and the scan timestamp (Hokuyo). 00353 long long int getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout = -1); 00354 00356 void querySensorConfig(); 00357 00359 char* laserReadlineAfter(char *buf, int len, const char *str, int timeout = -1); 00360 00362 bool checkSum(const char* buf, int buf_len); 00363 00365 void readData(LaserScan& scan, bool has_intensity, int timout = -1); 00366 00367 int dmin_; 00368 int dmax_; 00369 int ares_; 00370 int amin_; 00371 int amax_; 00372 int afrt_; 00373 int rate_; 00374 00375 int wrapped_; 00376 00377 unsigned int last_time_; 00378 00379 unsigned int time_repeat_count_; 00380 00381 long long offset_; 00382 00383 int laser_fd_; 00384 00385 std::string vendor_name_; 00386 std::string product_name_; 00387 std::string serial_number_; 00388 std::string protocol_version_; 00389 std::string firmware_version_; 00390 00391 char read_buf[256]; 00392 int read_buf_start; 00393 int read_buf_end; 00394 }; 00395 00396 } 00397 00398 #endif