c++/drivers/base/src/SickLIDAR.hh

Go to the documentation of this file.
00001 /*!
00002  * \file SickLIDAR.hh
00003  * \brief Defines the abstract parent class for defining
00004  *        a Sick LIDAR device driver.
00005  *
00006  * Code by Jason C. Derenick and Thomas H. Miller.
00007  * Contact derenick(at)lehigh(dot)edu
00008  *
00009  * The Sick LIDAR Matlab/C++ Toolbox
00010  * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
00011  * All rights reserved.
00012  *
00013  * This software is released under a BSD Open-Source License.
00014  * See http://sicktoolbox.sourceforge.net
00015  */
00016 
00017 /**
00018  * \mainpage The Sick LIDAR Matlab/C++ Toolbox 
00019  * \author Jason C. Derenick and Thomas H. Miller.
00020  * \version 1.0
00021  *
00022  * The Sick LIDAR Matlab/C++ Toolbox
00023  * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
00024  * All rights reserved.
00025  *
00026  * This software is released under a BSD Open-Source License.
00027  * See http://sicktoolbox.sourceforge.net
00028  */
00029 
00030 #ifndef SICK_LIDAR
00031 #define SICK_LIDAR
00032 
00033 /* Auto-generated header */
00034 #include "SickConfig.hh"
00035 
00036 /* Definition dependencies */
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 /* Associate the namespace */
00048 namespace SickToolbox {
00049 
00050   /**
00051    * \class SickLIDAR
00052    * \brief Provides an abstract parent for all Sick LIDAR devices
00053    */
00054   template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00055   class SickLIDAR {
00056 
00057   public:
00058     
00059     /** The primary constructor */
00060     SickLIDAR( );
00061 
00062     /** Indicates whether device is initialized */
00063     bool IsInitialized() { return _sick_initialized; }
00064     
00065     /** A virtual destructor */
00066     virtual ~SickLIDAR( );
00067 
00068   protected:
00069 
00070     /** Sick device file descriptor */
00071     int _sick_fd;
00072 
00073     /** A flag to indicated whether the device is properly initialized */
00074     bool _sick_initialized;
00075     
00076     /** A pointer to the driver's buffer monitor */
00077     SICK_MONITOR_CLASS *_sick_buffer_monitor;        
00078 
00079     /** Indicates whether the Sick buffer monitor is running */
00080     bool _sick_monitor_running;
00081 
00082     /** A method for setting up a general connection */
00083     virtual void _setupConnection( ) = 0;
00084     
00085     /** A method for tearing down a connection to the Sick */
00086     virtual void _teardownConnection( ) = 0;
00087 
00088     /** Starts the driver listening for messages */
00089     void _startListening( ) throw( SickThreadException );
00090 
00091     /** Stops the driver from listening */
00092     void _stopListening( )  throw( SickThreadException );
00093 
00094     /** Indicates whether there is a monitor currently running */
00095     bool _monitorRunning( ) const { return _sick_monitor_running; }
00096     
00097     /** Make the associated file descriptor non blocking */
00098     void _setBlockingIO( ) const throw ( SickIOException );
00099     
00100     /** Make the associated file descriptor non blocking */
00101     void _setNonBlockingIO( ) const throw ( SickIOException );
00102 
00103     /** Send a message to the Sick LD (allows specifying min time between transmitted bytes) */
00104     void _sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
00105       throw( SickIOException );
00106     
00107     /** Acquire the next message from the message container */
00108     void _recvMessage( SICK_MSG_CLASS &sick_message, const unsigned int timeout_value ) const throw ( SickTimeoutException );
00109 
00110     /** Search the stream for a payload with a particular "header" byte string */
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     /** An inline function for computing elapsed time */
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     /** Sends a request to the Sick and acquires looks for the reply */
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    * \brief Initializes the buffer monitor
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       /* Attempt to instantiate a new SickBufferMonitor for the device */
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    * \brief Destructor tears down buffer monitor
00149    */
00150   template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
00151   SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickLIDAR( ) {
00152     
00153     /* Deallocate the buffer monitor */
00154     if (_sick_buffer_monitor) {
00155       delete _sick_buffer_monitor;
00156     }
00157     
00158   }
00159 
00160   /**
00161    * \brief Activates the buffer monitor for the driver
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     /* Try to start the monitor */
00167     try {
00168       _sick_buffer_monitor->StartMonitor(_sick_fd);
00169     }
00170 
00171     /* Handle a thread exception */
00172     catch(SickThreadException &sick_thread_exception) {
00173       std::cerr << sick_thread_exception.what() << std::endl;
00174       throw;
00175     }
00176 
00177     /* Handle a thread exception */
00178     catch(...) {
00179       std::cerr << "SickLIDAR::_startListening: Unknown exception!!!" << std::endl;
00180       throw;
00181     }    
00182 
00183     /* Set the flag */
00184     _sick_monitor_running = true;
00185     
00186   }
00187 
00188   /**
00189    * \brief Activates the buffer monitor for the driver
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     /* Try to start the monitor */
00195     try {
00196       _sick_buffer_monitor->StopMonitor();
00197     }
00198 
00199     /* Handle a thread exception */
00200     catch(SickThreadException &sick_thread_exception) {
00201       std::cerr << sick_thread_exception.what() << std::endl;
00202       throw;
00203     }
00204 
00205     /* Handle a thread exception */
00206     catch(...) {
00207       std::cerr << "SickLIDAR::_stopListening: Unknown exception!!!" << std::endl;
00208       throw;
00209     }
00210 
00211     /* Reset the flag */
00212     _sick_monitor_running = false;
00213     
00214   }
00215 
00216   /**
00217    * \brief A simple method for setting blocking I/O
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     /* Read the flags */
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     /* Set the new flags  */
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    * \brief A simple method for setting non-blocking I/O
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     /* Read the flags */
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     /* Set the new flags */
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    * \brief Sends a message to the Sick device
00256    * \param &sick_message A reference to the well-formed message that is to be sent to the Sick
00257    * \param byte_interval Minimum time in microseconds between transmitted bytes
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     /* Copy the given message and get the message length */
00266     sick_message.GetMessage(message_buffer);
00267     unsigned int message_length = sick_message.GetMessageLength();
00268 
00269     /* Check whether a transmission delay between bytes is requested */
00270     if (byte_interval == 0) {
00271       
00272       /* Write the message to the stream */
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       /* Write the message to the unit one byte at a time */
00281       for (unsigned int i = 0; i < message_length; i++) {
00282         
00283         /* Write a single byte to the stream */
00284         if (write(_sick_fd,&message_buffer[i],1) != 1) {
00285           throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
00286         }
00287         
00288         /* Some time between bytes (Sick LMS 2xx likes this) */
00289         usleep(byte_interval);  
00290       }
00291 
00292     }    
00293 
00294   }
00295 
00296   /**
00297    * \brief Attempt to acquire the latest available message from the device
00298    * \param &sick_message A reference to the container that will hold the most recent message
00299    * \param timeout_value The time in secs to wait before throwing a timeout error
00300    * \return True if a new message was received, False otherwise
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     /* Timeval structs for handling timeouts */
00307     struct timeval beg_time, end_time;
00308 
00309     /* Acquire the elapsed time since epoch */
00310     gettimeofday(&beg_time,NULL);
00311     
00312     /* Check the shared object */
00313     while(!_sick_buffer_monitor->GetNextMessageFromMonitor(sick_message)) {    
00314       
00315       /* Sleep a little bit */
00316       usleep(1000);
00317     
00318       /* Check whether the allowed time has expired */
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    * \brief Attempt to acquire a message having a payload beginning w/ the given byte sequence
00330    * \param &sick_message A reference to the container that will hold the most recent message
00331    * \param *byte_sequence The byte sequence that is expected to lead off the payload in the packet (e.g. service codes, etc...)
00332    * \param byte_sequence_length The number of bytes in the given byte_sequence
00333    * \param timeout_value The time in secs to wait before throwing a timeout error
00334    * \return True if a new message was received, False otherwise
00335    *
00336    * NOTE: This method is intended to be a helper for _sendMessageAndGetReply
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     /* Define a buffer */
00345     uint8_t payload_buffer[SICK_MSG_CLASS::MESSAGE_PAYLOAD_MAX_LENGTH];
00346     
00347     /* Timeval structs for handling timeouts */
00348     struct timeval beg_time, end_time;    
00349     
00350     /* A container for the message */
00351     SICK_MSG_CLASS curr_message;
00352     
00353     /* Get the elapsed time since epoch */
00354     gettimeofday(&beg_time,NULL);
00355 
00356     /* Check until it is found or a timeout */
00357     for(;;) {
00358       
00359       /* Attempt to acquire the message */
00360       unsigned int i = 0;
00361       if (_sick_buffer_monitor->GetNextMessageFromMonitor(curr_message)) {      
00362         
00363         /* Extract the payload subregion */
00364         curr_message.GetPayloadSubregion(payload_buffer,0,byte_sequence_length-1);
00365         
00366         /* Match the byte sequence */
00367         for (i=0; (i < byte_sequence_length) && (payload_buffer[i] == byte_sequence[i]); i++);
00368 
00369         /* Our message was found! */
00370         if (i == byte_sequence_length) {
00371           sick_message = curr_message;
00372           break;
00373         }
00374         
00375       }
00376       
00377       /* Sleep a little bit */
00378       usleep(1000);     
00379       
00380       /* Check whether the allowed time has expired */
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    * \param sick_send_frame A sick frame to be sent to the LMS
00392    * \param sick_receive_frame A sick frame to hold the response (expected or unexpected) of the LMS
00393    * \param num_tries The number of times to send the frame in the event the LMS fails to reply
00394    * \param timeout The epoch to wait before considering a sent frame lost
00395    * \return True if the message was sent and the expected reply was received
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     /* Send the message for at most num_tries number of times */
00408     for(unsigned int i = 0; i < num_tries; i++) {
00409 
00410       try {
00411       
00412         /* Send the frame to the unit */
00413         _sendMessage(send_message,byte_interval);
00414         
00415         /* Wait for the reply! */
00416         _recvMessage(recv_message,byte_sequence,byte_sequence_length,timeout_value);
00417         
00418         /* message was found! */
00419         break;
00420 
00421       }
00422       
00423       /* Handle a timeout! */
00424       catch (SickTimeoutException &sick_timeout) {
00425         
00426         /* Check if it was found! */
00427         if (i == num_tries - 1) {
00428           throw SickTimeoutException("SickLIDAR::_sendMessageAndGetReply: Attempted max number of tries w/o success!");
00429         }
00430         
00431         /* Display the number of tries remaining! */
00432         std::cerr << sick_timeout.what() << " " << num_tries - i - 1  << " tries remaining" <<  std::endl;
00433         
00434       }
00435       
00436       /* Handle write buffer exceptions */
00437       catch (SickIOException &sick_io_error) {
00438         std::cerr << sick_io_error.what() << std::endl;
00439         throw;
00440       }
00441       
00442       /* A safety net */
00443       catch (...) {
00444         std::cerr << "SickLIDAR::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
00445         throw;
00446       }
00447       
00448     }
00449     
00450   }
00451   
00452 } /* namespace SickToolbox */
00453 
00454 #endif /* SICK_LIDAR */

Generated on Thu Mar 20 09:41:42 2008 for sicktoolbox-1.0 by  doxygen 1.5.1