00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef SICK_LIDAR
00031 #define SICK_LIDAR
00032
00033
00034 #include "SickConfig.hh"
00035
00036
00037 #include <new>
00038 #include <string>
00039 #include <iomanip>
00040 #include <iostream>
00041 #include <fcntl.h>
00042 #include <pthread.h>
00043 #include <arpa/inet.h>
00044 #include <sys/time.h>
00045 #include "SickException.hh"
00046
00047
00048 namespace SickToolbox {
00049
00050
00051
00052
00053
00054 template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00055 class SickLIDAR {
00056
00057 public:
00058
00059
00060 SickLIDAR( );
00061
00062
00063 bool IsInitialized() { return _sick_initialized; }
00064
00065
00066 virtual ~SickLIDAR( );
00067
00068 protected:
00069
00070
00071 int _sick_fd;
00072
00073
00074 bool _sick_initialized;
00075
00076
00077 SICK_MONITOR_CLASS *_sick_buffer_monitor;
00078
00079
00080 bool _sick_monitor_running;
00081
00082
00083 virtual void _setupConnection( ) = 0;
00084
00085
00086 virtual void _teardownConnection( ) = 0;
00087
00088
00089 void _startListening( ) throw( SickThreadException );
00090
00091
00092 void _stopListening( ) throw( SickThreadException );
00093
00094
00095 bool _monitorRunning( ) const { return _sick_monitor_running; }
00096
00097
00098 void _setBlockingIO( ) const throw ( SickIOException );
00099
00100
00101 void _setNonBlockingIO( ) const throw ( SickIOException );
00102
00103
00104 void _sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
00105 throw( SickIOException );
00106
00107
00108 void _recvMessage( SICK_MSG_CLASS &sick_message, const unsigned int timeout_value ) const throw ( SickTimeoutException );
00109
00110
00111 void _recvMessage( SICK_MSG_CLASS &sick_message,
00112 const uint8_t * const byte_sequence,
00113 const unsigned int byte_sequence_length,
00114 const unsigned int timeout_value ) const throw ( SickTimeoutException );
00115
00116
00117 double _computeElapsedTime( const struct timeval &beg_time, const struct timeval &end_time ) const { return ((end_time.tv_sec*1e6)+(end_time.tv_usec))-((beg_time.tv_sec*1e6)+beg_time.tv_usec); }
00118
00119
00120 virtual void _sendMessageAndGetReply( const SICK_MSG_CLASS &send_message,
00121 SICK_MSG_CLASS &recv_message,
00122 const uint8_t * const byte_sequence,
00123 const unsigned int byte_sequence_length,
00124 const unsigned int byte_interval,
00125 const unsigned int timeout_value,
00126 const unsigned int num_tries ) throw( SickTimeoutException, SickIOException);
00127
00128 };
00129
00130
00131
00132
00133 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00134 SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SickLIDAR( ) :
00135 _sick_fd(0), _sick_initialized(false), _sick_buffer_monitor(NULL), _sick_monitor_running(false) {
00136
00137 try {
00138
00139 _sick_buffer_monitor = new SICK_MONITOR_CLASS;
00140 }
00141 catch ( std::bad_alloc &allocation_exception ) {
00142 std::cerr << "SickLIDAR::SickLIDAR: Allocation error - " << allocation_exception.what() << std::endl;
00143 }
00144
00145 }
00146
00147
00148
00149
00150 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00151 SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickLIDAR( ) {
00152
00153
00154 if (_sick_buffer_monitor) {
00155 delete _sick_buffer_monitor;
00156 }
00157
00158 }
00159
00160
00161
00162
00163 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00164 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_startListening( ) throw( SickThreadException ) {
00165
00166
00167 try {
00168 _sick_buffer_monitor->StartMonitor(_sick_fd);
00169 }
00170
00171
00172 catch(SickThreadException &sick_thread_exception) {
00173 std::cerr << sick_thread_exception.what() << std::endl;
00174 throw;
00175 }
00176
00177
00178 catch(...) {
00179 std::cerr << "SickLIDAR::_startListening: Unknown exception!!!" << std::endl;
00180 throw;
00181 }
00182
00183
00184 _sick_monitor_running = true;
00185
00186 }
00187
00188
00189
00190
00191 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00192 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_stopListening( ) throw( SickThreadException ) {
00193
00194
00195 try {
00196 _sick_buffer_monitor->StopMonitor();
00197 }
00198
00199
00200 catch(SickThreadException &sick_thread_exception) {
00201 std::cerr << sick_thread_exception.what() << std::endl;
00202 throw;
00203 }
00204
00205
00206 catch(...) {
00207 std::cerr << "SickLIDAR::_stopListening: Unknown exception!!!" << std::endl;
00208 throw;
00209 }
00210
00211
00212 _sick_monitor_running = false;
00213
00214 }
00215
00216
00217
00218
00219 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00220 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setBlockingIO( ) const throw( SickIOException ) {
00221
00222
00223 int fd_flags = 0;
00224 if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) {
00225 throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!");
00226 }
00227
00228
00229 if(fcntl(_sick_fd,F_SETFL,fd_flags & (~O_NONBLOCK)) < 0) {
00230 throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!");
00231 }
00232
00233 }
00234
00235
00236
00237
00238 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00239 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setNonBlockingIO( ) const throw ( SickIOException ) {
00240
00241
00242 int fd_flags = 0;
00243 if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) {
00244 throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!");
00245 }
00246
00247
00248 if(fcntl(_sick_fd,F_SETFL,fd_flags | O_NONBLOCK) < 0) {
00249 throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!");
00250 }
00251
00252 }
00253
00254
00255
00256
00257
00258
00259 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00260 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
00261 throw( SickIOException ) {
00262
00263 uint8_t message_buffer[SICK_MSG_CLASS::MESSAGE_MAX_LENGTH] = {0};
00264
00265
00266 sick_message.GetMessage(message_buffer);
00267 unsigned int message_length = sick_message.GetMessageLength();
00268
00269
00270 if (byte_interval == 0) {
00271
00272
00273 if ((unsigned int)write(_sick_fd,message_buffer,message_length) != message_length) {
00274 throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
00275 }
00276
00277 }
00278 else {
00279
00280
00281 for (unsigned int i = 0; i < message_length; i++) {
00282
00283
00284 if (write(_sick_fd,&message_buffer[i],1) != 1) {
00285 throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
00286 }
00287
00288
00289 usleep(byte_interval);
00290 }
00291
00292 }
00293
00294 }
00295
00296
00297
00298
00299
00300
00301
00302 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00303 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message,
00304 const unsigned int timeout_value ) const throw ( SickTimeoutException ) {
00305
00306
00307 struct timeval beg_time, end_time;
00308
00309
00310 gettimeofday(&beg_time,NULL);
00311
00312
00313 while(!_sick_buffer_monitor->GetNextMessageFromMonitor(sick_message)) {
00314
00315
00316 usleep(1000);
00317
00318
00319 gettimeofday(&end_time,NULL);
00320 if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
00321 throw SickTimeoutException("SickLIDAR::_recvMessage: Timeout occurred!");
00322 }
00323
00324 }
00325
00326 }
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00339 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message,
00340 const uint8_t * const byte_sequence,
00341 const unsigned int byte_sequence_length,
00342 const unsigned int timeout_value ) const throw( SickTimeoutException ) {
00343
00344
00345 uint8_t payload_buffer[SICK_MSG_CLASS::MESSAGE_PAYLOAD_MAX_LENGTH];
00346
00347
00348 struct timeval beg_time, end_time;
00349
00350
00351 SICK_MSG_CLASS curr_message;
00352
00353
00354 gettimeofday(&beg_time,NULL);
00355
00356
00357 for(;;) {
00358
00359
00360 unsigned int i = 0;
00361 if (_sick_buffer_monitor->GetNextMessageFromMonitor(curr_message)) {
00362
00363
00364 curr_message.GetPayloadSubregion(payload_buffer,0,byte_sequence_length-1);
00365
00366
00367 for (i=0; (i < byte_sequence_length) && (payload_buffer[i] == byte_sequence[i]); i++);
00368
00369
00370 if (i == byte_sequence_length) {
00371 sick_message = curr_message;
00372 break;
00373 }
00374
00375 }
00376
00377
00378 usleep(1000);
00379
00380
00381 gettimeofday(&end_time,NULL);
00382 if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
00383 throw SickTimeoutException();
00384 }
00385
00386 }
00387
00388 }
00389
00390
00391
00392
00393
00394
00395
00396
00397 template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00398 void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessageAndGetReply( const SICK_MSG_CLASS &send_message,
00399 SICK_MSG_CLASS &recv_message,
00400 const uint8_t * const byte_sequence,
00401 const unsigned int byte_sequence_length,
00402 const unsigned int byte_interval,
00403 const unsigned int timeout_value,
00404 const unsigned int num_tries )
00405 throw( SickTimeoutException, SickIOException ) {
00406
00407
00408 for(unsigned int i = 0; i < num_tries; i++) {
00409
00410 try {
00411
00412
00413 _sendMessage(send_message,byte_interval);
00414
00415
00416 _recvMessage(recv_message,byte_sequence,byte_sequence_length,timeout_value);
00417
00418
00419 break;
00420
00421 }
00422
00423
00424 catch (SickTimeoutException &sick_timeout) {
00425
00426
00427 if (i == num_tries - 1) {
00428 throw SickTimeoutException("SickLIDAR::_sendMessageAndGetReply: Attempted max number of tries w/o success!");
00429 }
00430
00431
00432 std::cerr << sick_timeout.what() << " " << num_tries - i - 1 << " tries remaining" << std::endl;
00433
00434 }
00435
00436
00437 catch (SickIOException &sick_io_error) {
00438 std::cerr << sick_io_error.what() << std::endl;
00439 throw;
00440 }
00441
00442
00443 catch (...) {
00444 std::cerr << "SickLIDAR::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
00445 throw;
00446 }
00447
00448 }
00449
00450 }
00451
00452 }
00453
00454 #endif