SickLIDAR.hh
Go to the documentation of this file.
00001 
00030 #ifndef SICK_LIDAR
00031 #define SICK_LIDAR
00032 
00033 /* Definition dependencies */
00034 #include <new>
00035 #include <string>
00036 #include <iomanip>
00037 #include <iostream>
00038 #include <fcntl.h>
00039 #include <pthread.h>
00040 #include <arpa/inet.h>
00041 #include <sys/time.h>
00042 #include <unistd.h>
00043 #include "SickException.hh"
00044 
00045 /* Associate the namespace */
00046 namespace SickToolbox {
00047 
00052   template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00053   class SickLIDAR {
00054 
00055   public:
00056     
00058     SickLIDAR( );
00059 
00061     bool IsInitialized() { return _sick_initialized; }
00062     
00064     virtual ~SickLIDAR( );
00065 
00066   protected:
00067 
00069     int _sick_fd;
00070 
00072     bool _sick_initialized;
00073     
00075     SICK_MONITOR_CLASS *_sick_buffer_monitor;        
00076 
00078     bool _sick_monitor_running;
00079 
00081     virtual void _setupConnection( ) = 0;
00082     
00084     virtual void _teardownConnection( ) = 0;
00085 
00087     void _startListening( ) throw( SickThreadException );
00088 
00090     void _stopListening( )  throw( SickThreadException );
00091 
00093     bool _monitorRunning( ) const { return _sick_monitor_running; }
00094     
00096     void _setBlockingIO( ) const throw ( SickIOException );
00097     
00099     void _setNonBlockingIO( ) const throw ( SickIOException );
00100 
00102     void _sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
00103       throw( SickIOException );
00104     
00106     void _recvMessage( SICK_MSG_CLASS &sick_message, const unsigned int timeout_value ) const throw ( SickTimeoutException );
00107 
00109     void _recvMessage( SICK_MSG_CLASS &sick_message,
00110                        const uint8_t * const byte_sequence,
00111                        const unsigned int byte_sequence_length,
00112                        const unsigned int timeout_value ) const throw ( SickTimeoutException );
00113     
00115     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); }
00116     
00118     virtual void _sendMessageAndGetReply( const SICK_MSG_CLASS &send_message,
00119                                           SICK_MSG_CLASS &recv_message,
00120                                           const uint8_t * const byte_sequence,
00121                                           const unsigned int byte_sequence_length,
00122                                           const unsigned int byte_interval,
00123                                           const unsigned int timeout_value,
00124                                           const unsigned int num_tries ) throw( SickTimeoutException, SickIOException);
00125     
00126   };
00127 
00131   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00132   SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SickLIDAR( ) :
00133     _sick_fd(0), _sick_initialized(false), _sick_buffer_monitor(NULL), _sick_monitor_running(false) {
00134 
00135     try {
00136       /* Attempt to instantiate a new SickBufferMonitor for the device */
00137       _sick_buffer_monitor = new SICK_MONITOR_CLASS;
00138     }
00139     catch ( std::bad_alloc &allocation_exception ) {
00140       std::cerr << "SickLIDAR::SickLIDAR: Allocation error - " << allocation_exception.what() << std::endl;
00141     }
00142     
00143   }
00144 
00148   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00149   SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickLIDAR( ) {
00150     
00151     /* Deallocate the buffer monitor */
00152     if (_sick_buffer_monitor) {
00153       delete _sick_buffer_monitor;
00154     }
00155     
00156   }
00157 
00161   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00162   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_startListening( ) throw( SickThreadException ) {
00163 
00164     /* Try to start the monitor */
00165     try {
00166       _sick_buffer_monitor->StartMonitor(_sick_fd);
00167     }
00168 
00169     /* Handle a thread exception */
00170     catch(SickThreadException &sick_thread_exception) {
00171       std::cerr << sick_thread_exception.what() << std::endl;
00172       throw;
00173     }
00174 
00175     /* Handle a thread exception */
00176     catch(...) {
00177       std::cerr << "SickLIDAR::_startListening: Unknown exception!!!" << std::endl;
00178       throw;
00179     }    
00180 
00181     /* Set the flag */
00182     _sick_monitor_running = true;
00183     
00184   }
00185 
00189   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00190   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_stopListening( ) throw( SickThreadException ) {
00191 
00192     /* Try to start the monitor */
00193     try {
00194       _sick_buffer_monitor->StopMonitor();
00195     }
00196 
00197     /* Handle a thread exception */
00198     catch(SickThreadException &sick_thread_exception) {
00199       std::cerr << sick_thread_exception.what() << std::endl;
00200       throw;
00201     }
00202 
00203     /* Handle a thread exception */
00204     catch(...) {
00205       std::cerr << "SickLIDAR::_stopListening: Unknown exception!!!" << std::endl;
00206       throw;
00207     }
00208 
00209     /* Reset the flag */
00210     _sick_monitor_running = false;
00211     
00212   }
00213 
00217   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00218   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setBlockingIO( ) const throw( SickIOException ) {
00219 
00220     /* Read the flags */
00221     int fd_flags = 0;
00222     if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) {
00223       throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!");
00224     }
00225     
00226     /* Set the new flags  */
00227     if(fcntl(_sick_fd,F_SETFL,fd_flags & (~O_NONBLOCK)) < 0) {
00228       throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!");
00229     }
00230 
00231   }
00232   
00236   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00237   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setNonBlockingIO( ) const throw ( SickIOException ) {
00238 
00239     /* Read the flags */
00240     int fd_flags = 0;
00241     if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) {
00242       throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!");
00243     }
00244     
00245     /* Set the new flags */
00246     if(fcntl(_sick_fd,F_SETFL,fd_flags | O_NONBLOCK) < 0) {
00247       throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!");
00248     }
00249 
00250   }
00251   
00257   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00258   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
00259     throw( SickIOException ) {
00260 
00261     uint8_t message_buffer[SICK_MSG_CLASS::MESSAGE_MAX_LENGTH] = {0};
00262 
00263     /* Copy the given message and get the message length */
00264     sick_message.GetMessage(message_buffer);
00265     unsigned int message_length = sick_message.GetMessageLength();
00266 
00267     /* Check whether a transmission delay between bytes is requested */
00268     if (byte_interval == 0) {
00269       
00270       /* Write the message to the stream */
00271       if ((unsigned int)write(_sick_fd,message_buffer,message_length) != message_length) {      
00272         throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
00273       }
00274 
00275     }
00276     else {
00277       
00278       /* Write the message to the unit one byte at a time */
00279       for (unsigned int i = 0; i < message_length; i++) {
00280         
00281         /* Write a single byte to the stream */
00282         if (write(_sick_fd,&message_buffer[i],1) != 1) {
00283           throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
00284         }
00285         
00286         /* Some time between bytes (Sick LMS 2xx likes this) */
00287         usleep(byte_interval);  
00288       }
00289 
00290     }    
00291 
00292   }
00293 
00300   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00301   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message,
00302                                                                       const unsigned int timeout_value ) const throw ( SickTimeoutException ) {
00303 
00304     /* Timeval structs for handling timeouts */
00305     struct timeval beg_time, end_time;
00306 
00307     /* Acquire the elapsed time since epoch */
00308     gettimeofday(&beg_time,NULL);
00309     
00310     /* Check the shared object */
00311     while(!_sick_buffer_monitor->GetNextMessageFromMonitor(sick_message)) {    
00312       
00313       /* Sleep a little bit */
00314       usleep(1000);
00315     
00316       /* Check whether the allowed time has expired */
00317       gettimeofday(&end_time,NULL);    
00318       if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
00319         throw SickTimeoutException("SickLIDAR::_recvMessage: Timeout occurred!");
00320       }
00321       
00322     }
00323     
00324   }
00325 
00336   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00337   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message,
00338                                                                       const uint8_t * const byte_sequence,
00339                                                                       const unsigned int byte_sequence_length,
00340                                                                       const unsigned int timeout_value ) const throw( SickTimeoutException ) {
00341 
00342     /* Define a buffer */
00343     uint8_t payload_buffer[SICK_MSG_CLASS::MESSAGE_PAYLOAD_MAX_LENGTH];
00344     
00345     /* Timeval structs for handling timeouts */
00346     struct timeval beg_time, end_time;    
00347     
00348     /* A container for the message */
00349     SICK_MSG_CLASS curr_message;
00350     
00351     /* Get the elapsed time since epoch */
00352     gettimeofday(&beg_time,NULL);
00353 
00354     /* Check until it is found or a timeout */
00355     for(;;) {
00356       
00357       /* Attempt to acquire the message */
00358       unsigned int i = 0;
00359       if (_sick_buffer_monitor->GetNextMessageFromMonitor(curr_message)) {      
00360         
00361         /* Extract the payload subregion */
00362         curr_message.GetPayloadSubregion(payload_buffer,0,byte_sequence_length-1);
00363         
00364         /* Match the byte sequence */
00365         for (i=0; (i < byte_sequence_length) && (payload_buffer[i] == byte_sequence[i]); i++);
00366 
00367         /* Our message was found! */
00368         if (i == byte_sequence_length) {
00369           sick_message = curr_message;
00370           break;
00371         }
00372         
00373       }
00374       
00375       /* Sleep a little bit */
00376       usleep(1000);     
00377 
00378       /* Check whether the allowed time has expired */
00379       gettimeofday(&end_time,NULL);        
00380       if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
00381         throw SickTimeoutException();
00382       }      
00383       
00384     }
00385 
00386   }
00387 
00395   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00396   void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessageAndGetReply( const SICK_MSG_CLASS &send_message,
00397                                                                                  SICK_MSG_CLASS &recv_message,
00398                                                                                  const uint8_t * const byte_sequence,
00399                                                                                  const unsigned int byte_sequence_length,
00400                                                                                  const unsigned int byte_interval,
00401                                                                                  const unsigned int timeout_value,
00402                                                                                  const unsigned int num_tries ) 
00403                                                                                  throw( SickTimeoutException, SickIOException ) {
00404     
00405     /* Send the message for at most num_tries number of times */
00406     for(unsigned int i = 0; i < num_tries; i++) {
00407 
00408       try {
00409 
00410         /* Send the frame to the unit */
00411         _sendMessage(send_message,byte_interval);
00412         
00413         /* Wait for the reply! */
00414         _recvMessage(recv_message,byte_sequence,byte_sequence_length,timeout_value);
00415 
00416         /* message was found! */
00417         break;
00418 
00419       }
00420 
00421       /* Handle a timeout! */
00422       catch (SickTimeoutException &sick_timeout) {
00423         
00424         /* Check if it was found! */
00425         if (i == num_tries - 1) {
00426           throw SickTimeoutException("SickLIDAR::_sendMessageAndGetReply: Attempted max number of tries w/o success!");
00427         }
00428         
00429         /* Display the number of tries remaining! */
00430         std::cerr << sick_timeout.what() << " " << num_tries - i - 1  << " tries remaining" <<  std::endl;
00431         
00432       }
00433       
00434       /* Handle write buffer exceptions */
00435       catch (SickIOException &sick_io_error) {
00436         std::cerr << sick_io_error.what() << std::endl;
00437         throw;
00438       }
00439       
00440       /* A safety net */
00441       catch (...) {
00442         std::cerr << "SickLIDAR::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
00443         throw;
00444       }
00445       
00446     }
00447     
00448   }
00449   
00450 } /* namespace SickToolbox */
00451 
00452 #endif /* SICK_LIDAR */


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Thu Aug 27 2015 15:17:16