SickLMS1xx.cc
Go to the documentation of this file.
00001 
00016 /* Auto-generated header */
00017 #include <sicktoolbox/SickConfig.hh>
00018 
00019 /* Implementation dependencies */
00020 #include <string>
00021 #include <cmath>
00022 #include <cstdlib>
00023 #include <iostream>
00024 #include <iomanip>
00025 #include <sys/socket.h>       // for socket function definitions
00026 #include <arpa/inet.h>        // for sockaddr_in, inet_addr, and htons
00027 #include <sys/ioctl.h>        // for using ioctl functionality for the socket input buffer
00028 #include <unistd.h>           // for select functionality (e.g. FD_SET, etc...)
00029 #include <sys/types.h>        // for fd data types
00030 #include <sys/time.h>         // for select timeout parameter
00031 #include <fcntl.h>            // for getting file flags
00032 #include <pthread.h>          // for POSIX threads
00033 #include <sstream>            // for parsing ip addresses
00034 #include <vector>             // for returning the results of parsed strings
00035 #include <errno.h>            // for timing connect()
00036 #include <stdio.h>
00037 
00038 #include <sicktoolbox/SickLMS1xx.hh>
00039 #include <sicktoolbox/SickLMS1xxMessage.hh>
00040 #include <sicktoolbox/SickLMS1xxBufferMonitor.hh>
00041 #include <sicktoolbox/SickLMS1xxUtility.hh>   
00042 #include <sicktoolbox/SickException.hh>
00043 
00044 /* Associate the namespace */
00045 namespace SickToolbox {
00046 
00052   SickLMS1xx::SickLMS1xx( const std::string sick_ip_address, const uint16_t sick_tcp_port ) :
00053     SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage >( ),
00054     _sick_ip_address(sick_ip_address),
00055     _sick_tcp_port(sick_tcp_port),
00056     _sick_scan_format(SICK_LMS_1XX_SCAN_FORMAT_UNKNOWN),
00057     _sick_device_status(SICK_LMS_1XX_STATUS_UNKNOWN),
00058     _sick_temp_safe(false),
00059     _sick_streaming(false)
00060   {
00061     memset(&_sick_scan_config,0,sizeof(sick_lms_1xx_scan_config_t));
00062   }
00063 
00067   SickLMS1xx::~SickLMS1xx( ) { }
00068 
00072   void SickLMS1xx::Initialize( const bool disp_banner  ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException ) {
00073 
00074     if (disp_banner) {
00075       std::cout << "\t*** Attempting to initialize the Sick LMS 1xx..." << std::endl; 
00076     }
00077     
00078     try {
00079       
00080       /* Attempt to connect to the Sick LD */
00081       if (disp_banner) {
00082         std::cout << "\tAttempting to connect to Sick LMS 1xx @ " << _sick_ip_address << ":" << _sick_tcp_port << std::endl;
00083       }
00084       _setupConnection();
00085       if (disp_banner) {
00086         std::cout << "\t\tConnected to Sick LMS 1xx!" << std::endl;
00087       }
00088       
00089       /* Start the buffer monitor */
00090       if (disp_banner) {
00091         std::cout << "\tAttempting to start buffer monitor..." << std::endl;
00092       }
00093       _startListening();
00094       if (disp_banner) {
00095         std::cout << "\t\tBuffer monitor started!" << std::endl;
00096       }
00097       
00098       /* Ok, lets sync the driver with the Sick */
00099       if (disp_banner) {
00100         std::cout << "\tSyncing driver with Sick..." << std::endl;
00101       }
00102       _getSickScanConfig();
00103       _setAuthorizedClientAccessMode();
00104       if (disp_banner) {
00105         std::cout << "\t\tSuccess!" << std::endl;
00106         _printInitFooter();     
00107       }
00108         
00109     }
00110     
00111     catch(SickIOException &sick_io_exception) {
00112       std::cerr << sick_io_exception.what() << std::endl;
00113       throw;
00114     }
00115 
00116     catch(SickErrorException &sick_error_exception) {
00117       std::cerr << sick_error_exception.what() << std::endl;
00118       throw;
00119     }
00120     
00121     catch(SickThreadException &sick_thread_exception) {
00122       std::cerr << sick_thread_exception.what() << std::endl;
00123       throw;
00124     }
00125 
00126     catch(SickTimeoutException &sick_timeout_exception) {
00127       std::cerr << sick_timeout_exception.what() << std::endl;
00128       throw;
00129     }
00130 
00131     catch(...) {
00132       std::cerr << "SickLMS1xx::Initialize - Unknown exception!" << std::endl;
00133       throw;
00134     }
00135     
00136     //std::cout << "\t\tSynchronized!" << std::endl;
00137     _sick_initialized = true;
00138 
00139     /* Success */
00140   }
00141   
00148   void SickLMS1xx::SetSickScanFreqAndRes( const sick_lms_1xx_scan_freq_t scan_freq,
00149                                           const sick_lms_1xx_scan_res_t scan_res ) throw( SickTimeoutException, SickIOException, SickErrorException ) {
00150 
00151     /* Ensure the device has been initialized */
00152     if (!_sick_initialized) {
00153       throw SickIOException("SickLMS1xx::SetSickScanFreqAndRes: Device NOT Initialized!!!");
00154     }
00155 
00156     try {
00157 
00158       /* Is the device streaming? */
00159       if (_sick_streaming) {
00160         _stopStreamingMeasurements();
00161       }
00162 
00163       /* Set the desired configuration */
00164       _setSickScanConfig(scan_freq,
00165                          scan_res,
00166                          _sick_scan_config.sick_start_angle,
00167                          _sick_scan_config.sick_stop_angle);
00168 
00169     }
00170 
00171     /* Handle config exceptions */
00172     catch (SickErrorException &sick_error_exception) {
00173       std::cerr << sick_error_exception.what() << std::endl;
00174       throw;
00175     }
00176     
00177     /* Handle a timeout! */
00178     catch (SickTimeoutException &sick_timeout_exception) {
00179       std::cerr << sick_timeout_exception.what() << std::endl;
00180       throw;
00181     }
00182     
00183     /* Handle write buffer exceptions */
00184     catch (SickIOException &sick_io_exception) {
00185       std::cerr << sick_io_exception.what() << std::endl;
00186       throw;
00187     }
00188     
00189     /* A safety net */
00190     catch (...) {
00191       std::cerr << "SickLMS1xx::SetSickScanFreqAndRes: Unknown exception!!!" << std::endl;
00192       throw;
00193     }
00194     
00195   }
00196 
00201   sick_lms_1xx_scan_freq_t SickLMS1xx::GetSickScanFreq( ) const throw( SickIOException ) {
00202 
00203     /* Ensure the device has been initialized */
00204     if (!_sick_initialized) {
00205       throw SickIOException("SickLMS1xx::GetSickScanFreq: Device NOT Initialized!!!");
00206     }
00207 
00208     return IntToSickScanFreq(_convertSickFreqUnitsToHz(_sick_scan_config.sick_scan_freq));
00209     
00210   }
00211 
00216   sick_lms_1xx_scan_res_t SickLMS1xx::GetSickScanRes( ) const throw( SickIOException ) {
00217 
00218     /* Ensure the device has been initialized */
00219     if (!_sick_initialized) {
00220       throw SickIOException("SickLMS1xx::GetSickScanRes: Device NOT Initialized!!!");
00221     }
00222 
00223     return DoubleToSickScanRes(_convertSickAngleUnitsToDegs(_sick_scan_config.sick_scan_res));    
00224     
00225   }
00226 
00231   double SickLMS1xx::GetSickStartAngle( ) const throw( SickIOException ) {
00232 
00233     /* Ensure the device has been initialized */
00234     if (!_sick_initialized) {
00235       throw SickIOException("SickLMS1xx::GetSickStartAngle: Device NOT Initialized!!!");
00236     }
00237 
00238     return _convertSickAngleUnitsToDegs(_sick_scan_config.sick_start_angle);
00239     
00240   }
00241 
00246   double SickLMS1xx::GetSickStopAngle( ) const throw( SickIOException ) {
00247 
00248     /* Ensure the device has been initialized */
00249     if (!_sick_initialized) {
00250       throw SickIOException("SickLMS1xx::GetSickStopAngle: Device NOT Initialized!!!");
00251     }
00252 
00253     return _convertSickAngleUnitsToDegs(_sick_scan_config.sick_stop_angle);    
00254     
00255   }
00256 
00260   void SickLMS1xx::SetSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException ) {
00261 
00262     /* Ensure the device has been initialized */
00263     if (!_sick_initialized) {
00264       throw SickIOException("SickLMS1xx::SetSickScanDataFormat: Device NOT Initialized!!!");
00265     }
00266 
00267     /* If scan data format matches current format ignore it (perhaps a warning is in order?) */
00268     if (scan_format == _sick_scan_format) {
00269       return;
00270     }
00271     
00272     try {
00273       
00274       /* Is the device streaming? */
00275       if (_sick_streaming ) {
00276         _stopStreamingMeasurements();
00277       }
00278 
00279       std::cout << "\t*** Setting scan format " << _sickScanDataFormatToString(scan_format) << "..." << std::endl;
00280       
00281       /* Set the desired data format! */
00282       _setSickScanDataFormat(scan_format);
00283 
00284       std::cout << "\t\tSuccess!" << std::endl;
00285       
00286     }
00287     
00288     /* Handle a timeout! */
00289     catch (SickTimeoutException &sick_timeout_exception) {
00290       std::cerr << sick_timeout_exception.what() << std::endl;
00291       throw;
00292     }
00293     
00294     /* Handle write buffer exceptions */
00295     catch (SickIOException &sick_io_exception) {
00296       std::cerr << sick_io_exception.what() << std::endl;
00297       throw;
00298     }
00299     
00300     catch (...) {
00301       std::cerr << "SickLMS1xx::SetSickScanDataFormat: Unknown exception!!!" << std::endl;
00302       throw;
00303     }
00304 
00305     /* Success! */
00306     
00307   }
00308   
00316   void SickLMS1xx::GetSickMeasurements( unsigned int * const range_1_vals,
00317                                         unsigned int * const range_2_vals,
00318                                         unsigned int * const reflect_1_vals,
00319                                         unsigned int * const reflect_2_vals,
00320                                         unsigned int & num_measurements,
00321                                         unsigned int * const dev_status ) throw ( SickIOException, SickConfigException, SickTimeoutException ) {
00322     
00323     /* Ensure the device has been initialized */
00324     if (!_sick_initialized) {
00325       throw SickIOException("SickLMS1xx::GetSickMeasurements: Device NOT Initialized!!!");
00326     }
00327     
00328     try {
00329 
00330       /* Is the device already streaming? */
00331       if (!_sick_streaming) {
00332         _requestDataStream();
00333       }
00334 
00335     }
00336 
00337     /* Handle config exceptions */
00338     catch (SickConfigException &sick_config_exception) {
00339       std::cerr << sick_config_exception.what() << std::endl;
00340       throw;
00341     }
00342     
00343     /* Handle a timeout! */
00344     catch (SickTimeoutException &sick_timeout_exception) {
00345       std::cerr << sick_timeout_exception.what() << std::endl;
00346       throw;
00347     }
00348     
00349     /* Handle write buffer exceptions */
00350     catch (SickIOException &sick_io_exception) {
00351       std::cerr << sick_io_exception.what() << std::endl;
00352       throw;
00353     }
00354     
00355     catch (...) {
00356       std::cerr << "SickLMS1xx::GetSickMeasurements: Unknown exception!!!" << std::endl;
00357       throw;
00358     }
00359 
00360     /* Allocate receive message */
00361     SickLMS1xxMessage recv_message;
00362     
00363     try {
00364       
00365       /* Grab the next message from the stream */
00366       _recvMessage(recv_message);
00367 
00368     }
00369 
00370     /* Handle a timeout! */
00371     catch (SickTimeoutException &sick_timeout_exception) {
00372       std::cerr << sick_timeout_exception.what() << std::endl;
00373       throw;
00374     }
00375     
00376     catch (...) {
00377       std::cerr << "SickLMS1xx::GetSickMeasurements: Unknown exception!!!" << std::endl;
00378       throw;
00379     }
00380     
00381     /* Allocate a single buffer for payload contents */
00382     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH+1] = {0};
00383     
00384     recv_message.GetPayloadAsCStr((char *)payload_buffer);
00385 
00386     char * payload_str = NULL;
00387     unsigned int null_int = 0;
00388 
00389     /*
00390      * Acquire status
00391      */
00392     if (dev_status != NULL) {
00393 
00394       payload_str = (char *)&payload_buffer[16];      
00395       for (unsigned int i = 0; i < 3; i++) {
00396         payload_str = _convertNextTokenToUInt(payload_str,null_int);
00397       }
00398 
00399       /* Grab the contaimination value */
00400       _convertNextTokenToUInt(payload_str,*dev_status);
00401 
00402     }
00403 
00404     /*
00405      * Process DIST1
00406      */
00407     unsigned int num_dist_1_vals = 0;
00408     unsigned int num_dist_2_vals = 0;
00409     unsigned int num_rssi_1_vals = 0;
00410     unsigned int num_rssi_2_vals = 0;
00411     
00412     /* Locate DIST1 Section */
00413     if (range_1_vals != NULL) {
00414 
00415       const char * substr_dist_1 = "DIST1";
00416       unsigned int substr_dist_1_pos = 0;
00417       if (!_findSubString((char *)payload_buffer,substr_dist_1,recv_message.GetPayloadLength()+1,5,substr_dist_1_pos)) {
00418         throw SickIOException("SickLMS1xx::GetSickMeasurements: _findSubString() failed!");
00419       }
00420       
00421       /* Extract Num DIST1 Values */
00422       payload_str = (char *)&payload_buffer[substr_dist_1_pos+6];
00423       for (unsigned int i = 0; i < 4; i++) {
00424         payload_str = _convertNextTokenToUInt(payload_str,null_int);
00425       }
00426       
00427       payload_str = _convertNextTokenToUInt(payload_str,num_dist_1_vals);
00428       
00429       /* Grab the DIST1 values */
00430       for (unsigned int i = 0; i < num_dist_1_vals; i++) {
00431         payload_str = _convertNextTokenToUInt(payload_str,range_1_vals[i]);
00432       }
00433 
00434     }
00435       
00436     /*
00437      * Process DIST2
00438      */
00439     
00440     /* Locate DIST2 Section */
00441     if (range_2_vals != NULL) {
00442       
00443       const char * substr_dist_2 = "DIST2";
00444       unsigned int substr_dist_2_pos = 0;
00445       if (_findSubString((char *)payload_buffer,substr_dist_2,recv_message.GetPayloadLength()+1,5,substr_dist_2_pos)) {
00446 
00447         /* Extract Num DIST2 Values */
00448         payload_str = (char *)&payload_buffer[substr_dist_2_pos+6];
00449         for (unsigned int i = 0; i < 4; i++) {
00450           payload_str = _convertNextTokenToUInt(payload_str,null_int);
00451         }
00452         
00453         payload_str = _convertNextTokenToUInt(payload_str,num_dist_2_vals);
00454         
00455         /* Acquire the DIST2 values */
00456         for (unsigned int i = 0; i < num_dist_2_vals; i++) {
00457           payload_str = _convertNextTokenToUInt(payload_str,range_2_vals[i]);
00458         }
00459       }
00460       else {
00461         std::cerr << "SickLMS1xx::GetSickMeasurements: WARNING! It seems you are expecting double-pulse range values, which are not being streamed! ";
00462         std::cerr << "Use SetSickScanDataFormat to configure the LMS 1xx to stream these values - or - set the corresponding buffer input to NULL to avoid this warning." << std::endl; 
00463       }
00464         
00465     }
00466       
00467     /*
00468      * Process RSSI1
00469      */
00470 
00471     if (reflect_1_vals != NULL) {
00472     
00473       /* Locate RSSI1 Section */
00474       const char * substr_rssi_1 = "RSSI1";
00475       unsigned int substr_rssi_1_pos = 0;
00476       if (_findSubString((char *)payload_buffer,substr_rssi_1,recv_message.GetPayloadLength()+1,5,substr_rssi_1_pos)) {
00477       
00478         /* Extract Num RSSI1 Values */
00479         payload_str = (char *)&payload_buffer[substr_rssi_1_pos+6];
00480         for (unsigned int i = 0; i < 4; i++) {
00481           payload_str = _convertNextTokenToUInt(payload_str,null_int);
00482         }
00483         
00484         payload_str = _convertNextTokenToUInt(payload_str,num_rssi_1_vals);
00485         
00486         /* Grab the RSSI1 values */
00487         for (unsigned int i = 0; i < num_rssi_1_vals; i++) {
00488           payload_str = _convertNextTokenToUInt(payload_str,reflect_1_vals[i]);
00489         }
00490       }
00491       else {
00492         std::cerr << "SickLMS1xx::GetSickMeasurements: WARNING! It seems you are expecting single-pulse reflectivity values, which are not being streamed! ";
00493         std::cerr << "Use SetSickScanDataFormat to configure the LMS 1xx to stream these values - or - set the corresponding buffer input to NULL to avoid this warning." << std::endl; 
00494       }
00495           
00496     }
00497     
00498     /*
00499      * Process RSSI2
00500      */
00501 
00502     if (reflect_2_vals != NULL) {
00503     
00504       /* Locate RSSI2 Section */
00505       const char * substr_rssi_2 = "RSSI2";
00506       unsigned int substr_rssi_2_pos = 0;
00507       if (_findSubString((char *)payload_buffer,substr_rssi_2,recv_message.GetPayloadLength()+1,5,substr_rssi_2_pos)) {
00508     
00509         /* Extract Num RSSI1 Values */
00510         payload_str = (char *)&payload_buffer[substr_rssi_2_pos+6];
00511         for (unsigned int i = 0; i < 4; i++) {
00512           payload_str = _convertNextTokenToUInt(payload_str,null_int);
00513         }
00514         
00515         payload_str = _convertNextTokenToUInt(payload_str,num_rssi_2_vals);
00516         
00517         /* Grab the RSSI1 values */
00518         for (unsigned int i = 0; i < num_rssi_2_vals; i++) {
00519           payload_str = _convertNextTokenToUInt(payload_str,reflect_2_vals[i]);
00520         }
00521       }
00522       else {
00523         std::cerr << "SickLMS1xx::GetSickMeasurements: WARNING! It seems you are expecting double-pulse reflectivity values, which are not being streamed! ";
00524         std::cerr << "Use SetSickScanDataFormat to configure the LMS 1xx to stream these values - or - set the corresponding buffer input to NULL to avoid this warning." << std::endl; 
00525       }
00526 
00527     }
00528       
00529     /* Assign number of measurements */
00530     num_measurements = num_dist_1_vals;
00531     
00532     /* Success! */
00533     
00534   }
00535   
00539   void SickLMS1xx::Uninitialize( const bool disp_banner ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException ){
00540 
00541     /* Ensure the device has been initialized */
00542     if (!_sick_initialized) {
00543       throw SickIOException("SickLMS1xx::Uninitialize: Device NOT Initialized!!!");
00544     }
00545 
00546     if (disp_banner) {
00547       std::cout << std::endl << "\t*** Attempting to uninitialize the Sick LMS 1xx..." << std::endl; 
00548     }
00549       
00550     /* If necessary, tell the Sick LD to stop streaming data */
00551     try {
00552 
00553       /* Is the device streaming? */
00554       if (_sick_streaming) {
00555         _stopStreamingMeasurements(disp_banner);
00556       }
00557       
00558       /* Attempt to cancel the buffer monitor */
00559       if (disp_banner) {
00560         std::cout << "\tAttempting to cancel buffer monitor..." << std::endl;
00561       }
00562       _stopListening();
00563       if (disp_banner) {
00564         std::cout << "\t\tBuffer monitor canceled!" << std::endl;
00565       }
00566         
00567       /* Attempt to close the tcp connection */
00568       if (disp_banner) {
00569         std::cout << "\tClosing connection to Sick LMS 1xx..." << std::endl;
00570       }
00571       _teardownConnection();
00572 
00573       if (disp_banner) {
00574         std::cout << "\t\tConnection closed!" << std::endl;
00575         std::cout << "\t*** Uninit. complete - Sick LMS 1xx is now offline!" << std::endl; 
00576       }
00577       
00578     }
00579            
00580     /* Handle a timeout! */
00581     catch (SickTimeoutException &sick_timeout_exception) {
00582       std::cerr << sick_timeout_exception.what() << std::endl;
00583       throw;
00584     }
00585     
00586     /* Handle I/O exceptions */
00587     catch (SickIOException &sick_io_exception) {
00588       std::cerr << sick_io_exception.what() << std::endl;
00589       throw;
00590     }
00591     
00592     /* Handle a returned error code */
00593     catch (SickErrorException &sick_error_exception) {
00594       std::cerr << sick_error_exception.what() << std::endl;
00595       throw;
00596     }
00597 
00598     /* Handle a returned error code */
00599     catch (SickThreadException &sick_thread_exception) {
00600       std::cerr << sick_thread_exception.what() << std::endl;
00601       throw;
00602     }
00603     
00604     /* A safety net */
00605     catch (...) {
00606       std::cerr << "SickLMS::Uninitialize: Unknown exception!!!" << std::endl;
00607       throw;
00608     }  
00609 
00610     /* Mark the device as uninitialized */
00611     _sick_initialized = false;
00612   
00613     /* Success! */
00614   }
00615   
00619   SickLMS1xx::sick_lms_1xx_scan_freq_t SickLMS1xx::IntToSickScanFreq( const int scan_freq ) const {
00620 
00621     if (scan_freq == 25) {
00622       return SICK_LMS_1XX_SCAN_FREQ_25;
00623     }
00624     else if (scan_freq == 50) {
00625       return SICK_LMS_1XX_SCAN_FREQ_50;
00626     }
00627 
00628     return SICK_LMS_1XX_SCAN_FREQ_UNKNOWN;
00629     
00630   }
00631 
00635   int SickLMS1xx::SickScanFreqToInt( const sick_lms_1xx_scan_freq_t scan_freq ) const {
00636 
00637     switch(scan_freq) {
00638     case SICK_LMS_1XX_SCAN_FREQ_25:
00639       return 25;
00640     case SICK_LMS_1XX_SCAN_FREQ_50:
00641       return 50;
00642     default:
00643       return -1;
00644     }  
00645 
00646   }
00647   
00651   SickLMS1xx::sick_lms_1xx_scan_res_t SickLMS1xx::DoubleToSickScanRes( const double scan_res ) const {
00652 
00653     if (scan_res == 0.25) {
00654       return SICK_LMS_1XX_SCAN_RES_25;
00655     }
00656     else if (scan_res == 0.50) {
00657       return SICK_LMS_1XX_SCAN_RES_50;
00658     }
00659 
00660     return SICK_LMS_1XX_SCAN_RES_UNKNOWN;
00661     
00662   }  
00663 
00667   double SickLMS1xx::SickScanResToDouble( const sick_lms_1xx_scan_res_t scan_res ) const {
00668 
00669     switch(scan_res) {
00670     case SICK_LMS_1XX_SCAN_RES_25:
00671       return 0.25;
00672     case SICK_LMS_1XX_SCAN_RES_50:
00673       return 0.50;
00674     default:
00675       return -1;
00676     }  
00677 
00678   }
00679   
00683   void SickLMS1xx::_setupConnection( ) throw( SickIOException, SickTimeoutException ) {
00684 
00685     /* Create the TCP socket */
00686     if ((_sick_fd = socket(PF_INET,SOCK_STREAM,IPPROTO_TCP)) < 0) {
00687       throw SickIOException("SickLMS1xx::_setupConnection: socket() failed!");
00688     }
00689     
00690     /* Initialize the buffer */
00691     memset(&_sick_inet_address_info,0,sizeof(struct sockaddr_in));
00692   
00693     /* Setup the Sick LD inet address structure */
00694     _sick_inet_address_info.sin_family = AF_INET;                                  // Internet protocol address family
00695     _sick_inet_address_info.sin_port = htons(_sick_tcp_port);                      // TCP port number
00696     _sick_inet_address_info.sin_addr.s_addr = inet_addr(_sick_ip_address.c_str()); // Convert ip string to numerical address
00697 
00698     try {
00699 
00700       /* Set to non-blocking so we can time connect */
00701       _setNonBlockingIO();
00702     
00703       /* Try to connect to the Sick LD */
00704       int conn_return;
00705       if ((conn_return = connect(_sick_fd,(struct sockaddr *)&_sick_inet_address_info,sizeof(struct sockaddr_in))) < 0) {
00706 
00707         /* Check whether it is b/c it would block */
00708         if (errno != EINPROGRESS) {     
00709           throw SickIOException("SickLMS1xx::_setupConnection: connect() failed!");
00710         }
00711 
00712         /* Use select to wait on the socket */
00713         int valid_opt = 0;
00714         int num_active_files = 0;
00715         struct timeval timeout_val;                                  // This structure will be used for setting our timeout values
00716         fd_set file_desc_set;                                        // File descriptor set for monitoring I/O
00717     
00718         /* Initialize and set the file descriptor set for select */
00719         FD_ZERO(&file_desc_set);
00720         FD_SET(_sick_fd,&file_desc_set);
00721 
00722         /* Setup the timeout structure */
00723         memset(&timeout_val,0,sizeof(timeout_val));                  // Initialize the buffer
00724         timeout_val.tv_usec = DEFAULT_SICK_LMS_1XX_CONNECT_TIMEOUT;  // Wait for specified time before throwing a timeout
00725       
00726         /* Wait for the OS to tell us that the connection is established! */
00727         num_active_files = select(getdtablesize(),0,&file_desc_set,0,&timeout_val);
00728       
00729         /* Figure out what to do based on the output of select */
00730         if (num_active_files > 0) {
00731         
00732           /* This is just a sanity check */
00733           if (!FD_ISSET(_sick_fd,&file_desc_set)) {
00734             throw SickIOException("SickLMS1xx::_setupConnection: Unexpected file descriptor!");
00735           }       
00736 
00737           /* Check for any errors on the socket - just to be sure */
00738           socklen_t len = sizeof(int);
00739           if (getsockopt(_sick_fd,SOL_SOCKET,SO_ERROR,(void*)(&valid_opt),&len) < 0) {      
00740             throw SickIOException("SickLMS1xx::_setupConnection: getsockopt() failed!");
00741           } 
00742 
00743           /* Check whether the opt value indicates error */
00744           if (valid_opt) { 
00745             throw SickIOException("SickLMS1xx::_setupConnection: socket error on connect()!");
00746           }
00747           
00748         }
00749         else if (num_active_files == 0) {
00750         
00751           /* A timeout has occurred! */
00752           throw SickTimeoutException("SickLMS1xx::_setupConnection: select() timeout!");
00753 
00754         }
00755         else {
00756         
00757           /* An error has occurred! */
00758           throw SickIOException("SickLMS1xx::_setupConnection: select() failed!");      
00759 
00760         }
00761 
00762       }
00763 
00764       /* Restore blocking IO */
00765       _setBlockingIO(); 
00766         
00767     }
00768 
00769     catch(SickIOException &sick_io_exception) {
00770       std::cerr << sick_io_exception.what() << std::endl;
00771       throw;
00772     }
00773 
00774     catch(SickTimeoutException &sick_timeout_exception) {
00775       std::cerr << sick_timeout_exception.what() << std::endl;
00776       throw;
00777     }
00778     
00779     catch(...) {
00780       std::cerr << "SickLMS1xx::_setupConnection - Unknown exception occurred!" << std::endl;
00781       throw;
00782     }
00783 
00784     /* Success */
00785   }
00786 
00790   void SickLMS1xx::_reinitialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException ) {
00791 
00792     try {
00793 
00794       //std::cout << "\t*** Reinitializing Sick LMS 1xx..." << std::endl;
00795       
00796       /* Uninitialize the device */
00797       Uninitialize(false);
00798 
00799       /* Initialize the device */
00800       Initialize(false);
00801 
00802       //std::cout << "\t\tSuccess!" << std::endl;
00803       
00804     }
00805 
00806     /* Handle a timeout! */
00807     catch (SickTimeoutException &sick_timeout_exception) {
00808       std::cerr << sick_timeout_exception.what() << std::endl;
00809       throw;
00810     }
00811     
00812     /* Handle write buffer exceptions */
00813     catch (SickIOException &sick_io_exception) {
00814       std::cerr << sick_io_exception.what() << std::endl;
00815       throw;
00816     }
00817 
00818     /* Handle a timeout! */
00819     catch (SickThreadException &sick_thread_exception) {
00820       std::cerr << sick_thread_exception.what() << std::endl;
00821       throw;
00822     }
00823 
00824     /* Handle a timeout! */
00825     catch (SickErrorException &sick_error_exception) {
00826       std::cerr << sick_error_exception.what() << std::endl;
00827       throw;
00828     }
00829     
00830     catch (...) {
00831       std::cerr << "SickLMS1xx::_reinitialize: Unknown exception!!!" << std::endl;
00832       throw;
00833     }
00834     
00835   }
00836 
00840   void SickLMS1xx::_teardownConnection( ) throw( SickIOException ) {
00841      
00842      /* Close the socket! */
00843      if (close(_sick_fd) < 0) {
00844        throw SickIOException("SickLMS1xx::_teardownConnection: close() failed!");
00845      }
00846      
00847    }
00848   
00852   void SickLMS1xx::_updateSickStatus( ) throw( SickTimeoutException, SickIOException ) {
00853 
00854     /* Allocate a single buffer for payload contents */
00855     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00856 
00857     /* Set the command type */
00858     payload_buffer[0] = 's';
00859     payload_buffer[1] = 'R';
00860     payload_buffer[2] = 'N';
00861     
00862     payload_buffer[3] = ' ';
00863 
00864     /* Set the command */
00865     payload_buffer[4] = 'S';
00866     payload_buffer[5] = 'T';
00867     payload_buffer[6] = 'l';
00868     payload_buffer[7] = 'm';
00869     payload_buffer[8] = 's';
00870 
00871     /* Construct command message */
00872     SickLMS1xxMessage send_message(payload_buffer,9);
00873 
00874     /* Setup container for recv message */
00875     SickLMS1xxMessage recv_message;
00876 
00877     /* Send message and get reply using parent's method */
00878     try {
00879       
00880       _sendMessageAndGetReply(send_message, recv_message, "sRA", "STlms");
00881 
00882     }
00883         
00884     /* Handle a timeout! */
00885     catch (SickTimeoutException &sick_timeout_exception) {
00886       std::cerr << sick_timeout_exception.what() << std::endl;
00887       throw;
00888     }
00889     
00890     /* Handle write buffer exceptions */
00891     catch (SickIOException &sick_io_exception) {
00892       std::cerr << sick_io_exception.what() << std::endl;
00893       throw;
00894     }
00895     
00896     /* A safety net */
00897     catch (...) {
00898       std::cerr << "SickLMS1xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
00899       throw;
00900     }
00901     
00902     /* Reset the buffer (not necessary, but its better to do so just in case) */
00903     memset(payload_buffer,0,9);
00904   
00905     /* Extract the message payload */
00906     recv_message.GetPayload(payload_buffer);
00907 
00908     _sick_device_status = _intToSickStatus(atoi((char *)&payload_buffer[10]));
00909     _sick_temp_safe = (bool)atoi((char *)&payload_buffer[12]);
00910 
00911     /* Success */
00912 
00913   }
00914 
00918   void SickLMS1xx::_getSickScanConfig( ) throw( SickTimeoutException, SickIOException ) {
00919                                       
00920     /* Allocate a single buffer for payload contents */
00921     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00922 
00923     /* Set the command type */
00924     payload_buffer[0]  = 's';
00925     payload_buffer[1]  = 'R';
00926     payload_buffer[2]  = 'N';
00927     
00928     payload_buffer[3]  = ' ';
00929 
00930     /* Set the command */
00931     payload_buffer[4]  = 'L';
00932     payload_buffer[5]  = 'M';
00933     payload_buffer[6]  = 'P';
00934     payload_buffer[7]  = 's';
00935     payload_buffer[8]  = 'c';
00936     payload_buffer[9]  = 'a';
00937     payload_buffer[10] = 'n';
00938     payload_buffer[11] = 'c';
00939     payload_buffer[12] = 'f';
00940     payload_buffer[13] = 'g';    
00941 
00942     /* Construct command message */
00943     SickLMS1xxMessage send_message(payload_buffer,14);
00944 
00945     /* Setup container for recv message */
00946     SickLMS1xxMessage recv_message;
00947 
00948     /* Send message and get reply using parent's method */
00949     try {
00950 
00951       _sendMessageAndGetReply(send_message, recv_message, "sRA", "LMPscancfg");
00952 
00953     }
00954         
00955     /* Handle a timeout! */
00956     catch (SickTimeoutException &sick_timeout_exception) {
00957       std::cerr << sick_timeout_exception.what() << std::endl;
00958       throw;
00959     }
00960     
00961     /* Handle write buffer exceptions */
00962     catch (SickIOException &sick_io_exception) {
00963       std::cerr << sick_io_exception.what() << std::endl;
00964       throw;
00965     }
00966     
00967     /* A safety net */
00968     catch (...) {
00969       std::cerr << "SickLMS1xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
00970       throw;
00971     }
00972     
00973     /* Reset the buffer (not necessary, but its better to do so just in case) */
00974     memset(payload_buffer,0,14);
00975   
00976     /* Extract the message payload */
00977     recv_message.GetPayloadAsCStr((char *)payload_buffer);
00978 
00979     /* Utility variables */
00980     uint32_t scan_freq = 0, scan_res = 0;
00981     uint32_t sick_start_angle = 0, sick_stop_angle = 0;
00982 
00983     /*
00984      * Grab the scanning frequency
00985      */
00986     const char * token = NULL;
00987     if ((token = strtok((char *)&payload_buffer[15]," ")) == NULL) {
00988       throw SickIOException("SickLMS1xx::_getSickConfig: strtok() failed!");
00989     }
00990 
00991     if (sscanf(token,"%x",&scan_freq) == EOF) {
00992       throw SickIOException("SickLMS1xx::_getSickConfig: sscanf() failed!");
00993     }
00994 
00995     sick_lms_1xx_scan_freq_t sick_scan_freq;
00996     sick_scan_freq = (sick_lms_1xx_scan_freq_t)sick_lms_1xx_to_host_byte_order(scan_freq);
00997 
00998     /* Ignore the number of segments value (its always 1 for the LMS 1xx) */
00999     if ((token = strtok(NULL," ")) == NULL) {
01000       throw SickIOException("SickLMS1xx::_getSickConfig: strtok() failed!");
01001     }
01002 
01003     /*
01004      * Grab the angular resolution
01005      */    
01006     if ((token = strtok(NULL," ")) == NULL) {
01007       throw SickIOException("SickLMS1xx::_getSickConfig: strtok() failed!");
01008     }
01009     
01010     if (sscanf(token,"%x",&scan_res) == EOF) {
01011       throw SickIOException("SickLMS1xx::_getSickConfig: sscanf() failed!");
01012     }
01013 
01014     sick_lms_1xx_scan_res_t sick_scan_res;
01015     sick_scan_res = (sick_lms_1xx_scan_res_t)sick_lms_1xx_to_host_byte_order(scan_res);
01016 
01017     /*
01018      * Grab the start angle
01019      */    
01020     if ((token = strtok(NULL," ")) == NULL) {
01021       throw SickIOException("SickLMS1xx::_getSickConfig: strtok() failed!");
01022     }
01023     
01024     if (sscanf(token,"%x",&sick_start_angle) == EOF) {
01025       throw SickIOException("SickLMS1xx::_getSickConfig: sscanf() failed!");
01026     }
01027     
01028     sick_start_angle = sick_lms_1xx_to_host_byte_order(sick_start_angle);
01029 
01030     /*
01031      * Grab the stop angle
01032      */    
01033     if ((token = strtok(NULL," ")) == NULL) {
01034       throw SickIOException("SickLMS1xx::_getSickConfig: strtok() failed!");
01035     }
01036     
01037     if (sscanf(token,"%x",&sick_stop_angle) == EOF) {
01038       throw SickIOException("SickLMS1xx::_getSickConfig: sscanf() failed!");
01039     }
01040     
01041     sick_stop_angle = sick_lms_1xx_to_host_byte_order(sick_stop_angle);
01042 
01043     /*
01044      * Assign the config values!
01045      */
01046     _sick_scan_config.sick_scan_freq = sick_scan_freq;
01047     _sick_scan_config.sick_scan_res = sick_scan_res;
01048     _sick_scan_config.sick_start_angle = sick_start_angle;
01049     _sick_scan_config.sick_stop_angle = sick_stop_angle;
01050     
01051     /* Success */
01052 
01053   }  
01054 
01063   void SickLMS1xx::_setSickScanConfig( const sick_lms_1xx_scan_freq_t scan_freq,
01064                                        const sick_lms_1xx_scan_res_t scan_res,
01065                                        const int start_angle, const int stop_angle ) throw( SickTimeoutException, SickIOException, SickErrorException ) {
01066 
01067     /* Verify valid inputs */
01068     if (!_validScanArea(start_angle, stop_angle)) {
01069       throw SickConfigException("SickLMS1xx::_setSickScanConfig - Invalid Sick LMS 1xx Scan Area!");
01070     }
01071     
01072     /* Allocate a single buffer for payload contents */
01073     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01074 
01075     std::cout << std::endl << "\t*** Attempting to configure device..." << std::endl;
01076     
01077     /* Set the command type */
01078     payload_buffer[0]  = 's';
01079     payload_buffer[1]  = 'M';
01080     payload_buffer[2]  = 'N';
01081     
01082     payload_buffer[3]  = ' ';
01083 
01084     /* Set the command */
01085     payload_buffer[4]  = 'm';
01086     payload_buffer[5]  = 'L';
01087     payload_buffer[6]  = 'M';
01088     payload_buffer[7]  = 'P';
01089     payload_buffer[8]  = 's';
01090     payload_buffer[9]  = 'e';
01091     payload_buffer[10] = 't';
01092     payload_buffer[11] = 's';
01093     payload_buffer[12] = 'c';
01094     payload_buffer[13] = 'a';
01095     payload_buffer[14] = 'n';
01096     payload_buffer[15] = 'c';
01097     payload_buffer[16] = 'f';
01098     payload_buffer[17] = 'g';
01099 
01100     payload_buffer[18] = ' ';    
01101 
01102     /* Desired scanning frequency */
01103     std::string freq_str = int_to_str((int)scan_freq);
01104     
01105     payload_buffer[19] = '+';
01106     
01107     for (int i = 0; i < 4; i++) {
01108       payload_buffer[20+i] = (uint8_t)((freq_str.c_str())[i]);
01109     }
01110 
01111     payload_buffer[24] = ' ';    
01112 
01113     /* Desired number of segments (always 1) */
01114     payload_buffer[25] = '+';
01115     payload_buffer[26] = '1';
01116 
01117     payload_buffer[27] = ' ';    
01118     
01119     /* Desired angular resolution */
01120     std::string res_str = int_to_str((int)scan_res);
01121     
01122     payload_buffer[28] = '+';   
01123     
01124     for (int i = 0; i < 4; i++) {
01125       payload_buffer[29+i] = (uint8_t)((res_str.c_str())[i]);
01126     }
01127 
01128     payload_buffer[33] = ' ';
01129 
01130     /* Desired starting angle */
01131     std::string start_angle_str = int_to_str(start_angle);
01132 
01133     unsigned int idx = 34;
01134     if (start_angle >= 0) {
01135       payload_buffer[idx] = '+';
01136       idx++;
01137     }
01138 
01139     for (int i = 0; i < start_angle_str.length(); idx++, i++) {
01140       payload_buffer[idx] = (uint8_t)(start_angle_str.c_str())[i];
01141     }
01142 
01143     payload_buffer[idx] = ' ';
01144     idx++;
01145 
01146     /* Desired stopping angle */
01147     std::string stop_angle_str = int_to_str(stop_angle);
01148 
01149     if (stop_angle >= 0) {
01150       payload_buffer[idx] = '+';
01151       idx++;
01152     }
01153     
01154     for (int i = 0; i < stop_angle_str.length(); idx++, i++) {
01155       payload_buffer[idx] = (uint8_t)(stop_angle_str.c_str())[i];
01156     }
01157         
01158     /* Construct command message */
01159     SickLMS1xxMessage send_message(payload_buffer,idx);
01160 
01161     /* Setup container for recv message */
01162     SickLMS1xxMessage recv_message;
01163 
01164     try {
01165 
01166       /* Send message and get reply */
01167       _sendMessageAndGetReply(send_message, recv_message, "sAN", "mLMPsetscancfg");
01168 
01169     }
01170         
01171     /* Handle a timeout! */
01172     catch (SickTimeoutException &sick_timeout_exception) {
01173       std::cerr << sick_timeout_exception.what() << std::endl;
01174       throw;
01175     }
01176     
01177     /* Handle write buffer exceptions */
01178     catch (SickIOException &sick_io_exception) {
01179       std::cerr << sick_io_exception.what() << std::endl;
01180       throw;
01181     }
01182     
01183     /* A safety net */
01184     catch (...) {
01185       std::cerr << "SickLMS1xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
01186       throw;
01187     }
01188     
01189     /* Reset the buffer (not necessary, but its better to do so just in case) */
01190     memset(payload_buffer,0,SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH);
01191   
01192     /* Extract the message payload */
01193     recv_message.GetPayload(payload_buffer);
01194     
01195     /* Check if it worked... */
01196     if (payload_buffer[19] != '0') {
01197         throw SickErrorException("SickLMS1xx::_setSickScanConfig: " + _intToSickConfigErrorStr(atoi((char *)&payload_buffer[19])));
01198     }
01199 
01200     std::cout << "\t\tDevice configured!" << std::endl << std::endl;
01201     
01202     /* Update the scan configuration! */
01203     try {
01204 
01205       _getSickScanConfig();
01206       
01207     }
01208 
01209     /* Handle a timeout! */
01210     catch (SickTimeoutException &sick_timeout_exception) {
01211       std::cerr << sick_timeout_exception.what() << std::endl;
01212       throw;
01213     }
01214     
01215     /* Handle write buffer exceptions */
01216     catch (SickIOException &sick_io_exception) {
01217       std::cerr << sick_io_exception.what() << std::endl;
01218       throw;
01219     }
01220 
01221     /* A safety net */
01222     catch (...) {
01223       std::cerr << "SickLMS1xx::_setSickScanConfig: Unknown exception!!!" << std::endl;
01224       throw;
01225     }
01226 
01227     /* Success! */   
01228     _printSickScanConfig();
01229 
01230   }
01231 
01232   
01236   void SickLMS1xx::_setAuthorizedClientAccessMode() throw( SickTimeoutException, SickErrorException, SickIOException ) {
01237 
01238     /* Allocate a single buffer for payload contents */
01239     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01240     
01241     /* Set the command type */
01242     payload_buffer[0]  = 's';
01243     payload_buffer[1]  = 'M';
01244     payload_buffer[2]  = 'N';
01245     
01246     payload_buffer[3]  = ' ';
01247 
01248     /* Set the command */
01249     payload_buffer[4]  = 'S';
01250     payload_buffer[5]  = 'e';
01251     payload_buffer[6]  = 't';
01252     payload_buffer[7]  = 'A';
01253     payload_buffer[8]  = 'c';
01254     payload_buffer[9]  = 'c';
01255     payload_buffer[10] = 'e';
01256     payload_buffer[11] = 's';
01257     payload_buffer[12] = 's';
01258     payload_buffer[13] = 'M';
01259     payload_buffer[14] = 'o';
01260     payload_buffer[15] = 'd';
01261     payload_buffer[16] = 'e';    
01262 
01263     payload_buffer[17] = ' ';
01264     
01265     /* Set as authorized client */
01266     payload_buffer[18] = '0';
01267     payload_buffer[19] = '3';
01268 
01269     payload_buffer[20] = ' ';
01270 
01271     /* Encoded value for client */
01272     payload_buffer[21] = 'F';
01273     payload_buffer[22] = '4';
01274     payload_buffer[23] = '7';
01275     payload_buffer[24] = '2';
01276     payload_buffer[25] = '4';
01277     payload_buffer[26] = '7';
01278     payload_buffer[27] = '4';
01279     payload_buffer[28] = '4';
01280 
01281     /* Construct command message */
01282     SickLMS1xxMessage send_message(payload_buffer,29);
01283 
01284     /* Setup container for recv message */
01285     SickLMS1xxMessage recv_message;
01286 
01287     /* Send message and get reply using parent's method */
01288     try {
01289       
01290       _sendMessageAndGetReply(send_message, recv_message, "sAN", "SetAccessMode");
01291 
01292     }
01293         
01294     /* Handle a timeout! */
01295     catch (SickTimeoutException &sick_timeout_exception) {
01296       std::cerr << sick_timeout_exception.what() << std::endl;
01297       throw;
01298     }
01299     
01300     /* Handle write buffer exceptions */
01301     catch (SickIOException &sick_io_exception) {
01302       std::cerr << sick_io_exception.what() << std::endl;
01303       throw;
01304     }
01305     
01306     /* A safety net */
01307     catch (...) {
01308       std::cerr << "SickLMS1xx::_setAuthorizedClientAccessMode: Unknown exception!!!" << std::endl;
01309       throw;
01310     }
01311     
01312     /* Reset the buffer (not necessary, but its better to do so just in case) */
01313     memset(payload_buffer,0,29);
01314     
01315     /* Extract the message payload */
01316     recv_message.GetPayload(payload_buffer);
01317 
01318     /* Check Response */
01319     if (payload_buffer[18] != '1') {
01320       throw SickErrorException("SickLMS1xx::_setAuthorizedClientAccessMode: Setting Access Mode Failed!");    
01321     }
01322 
01323     /* Success! Woohoo! */
01324     
01325   }
01326 
01330   void SickLMS1xx::_writeToEEPROM( ) throw( SickTimeoutException, SickIOException ) {
01331 
01332     /* Allocate a single buffer for payload contents */
01333     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01334     
01335     /* Set the command type */
01336     payload_buffer[0]  = 's';
01337     payload_buffer[1]  = 'M';
01338     payload_buffer[2]  = 'N';
01339     
01340     payload_buffer[3]  = ' ';
01341 
01342     /* Set the command */
01343     payload_buffer[4]  = 'm';
01344     payload_buffer[5]  = 'E';
01345     payload_buffer[6]  = 'E';
01346     payload_buffer[7]  = 'w';
01347     payload_buffer[8]  = 'r';
01348     payload_buffer[9]  = 'i';
01349     payload_buffer[10] = 't';
01350     payload_buffer[11] = 'e';
01351     payload_buffer[12] = 'a';
01352     payload_buffer[13] = 'l';
01353     payload_buffer[14] = 'l';
01354 
01355     /* Construct command message */
01356     SickLMS1xxMessage send_message(payload_buffer,15);
01357 
01358     /* Setup container for recv message */
01359     SickLMS1xxMessage recv_message;
01360 
01361     try {
01362 
01363       /* Send message and get reply */      
01364       _sendMessageAndGetReply(send_message, recv_message, "sAN", "mEEwriteall");
01365 
01366     }
01367         
01368     /* Handle a timeout! */
01369     catch (SickTimeoutException &sick_timeout_exception) {
01370       std::cerr << sick_timeout_exception.what() << std::endl;
01371       throw;
01372     }
01373     
01374     /* Handle write buffer exceptions */
01375     catch (SickIOException &sick_io_exception) {
01376       std::cerr << sick_io_exception.what() << std::endl;
01377       throw;
01378     }
01379     
01380     /* A safety net */
01381     catch (...) {
01382       std::cerr << "SickLMS1xx::_writeToEEPROM: Unknown exception!!!" << std::endl;
01383       throw;
01384     }
01385     
01386     /* Reset the buffer (not necessary, but its better to do so just in case) */
01387     memset(payload_buffer,0,15);
01388     
01389     /* Extract the message payload */
01390     recv_message.GetPayload(payload_buffer);
01391 
01392     /* Check Response */
01393     if (payload_buffer[13] != '1') {
01394       throw SickIOException("SickLMS1xx::_writeToEEPROM: Failed to Write Data!");    
01395     }
01396 
01397     /* Success! Woohoo! */
01398     
01399   }
01400 
01404   void SickLMS1xx::_startMeasuring( ) throw( SickTimeoutException, SickIOException ) {
01405 
01406     /* Allocate a single buffer for payload contents */
01407     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01408     
01409     /* Set the command type */
01410     payload_buffer[0]  = 's';
01411     payload_buffer[1]  = 'M';
01412     payload_buffer[2]  = 'N';
01413     payload_buffer[3]  = ' ';
01414     
01415     /* Set the command */
01416     payload_buffer[4]  = 'L';
01417     payload_buffer[5]  = 'M';
01418     payload_buffer[6]  = 'C';
01419     payload_buffer[7]  = 's';
01420     payload_buffer[8]  = 't';
01421     payload_buffer[9]  = 'a';
01422     payload_buffer[10] = 'r';
01423     payload_buffer[11] = 't';
01424     payload_buffer[12] = 'm';
01425     payload_buffer[13] = 'e';
01426     payload_buffer[14] = 'a';
01427     payload_buffer[15] = 's';    
01428 
01429     /* Construct command message */
01430     SickLMS1xxMessage send_message(payload_buffer,16);
01431     
01432     /* Setup container for recv message */
01433     SickLMS1xxMessage recv_message;
01434 
01435     try {
01436 
01437       /* Send message and get reply */      
01438       _sendMessageAndGetReply(send_message, recv_message, "sAN", "LMCstartmeas");
01439       
01440     }
01441     
01442     /* Handle a timeout! */
01443     catch (SickTimeoutException &sick_timeout_exception) {
01444       std::cerr << sick_timeout_exception.what() << std::endl;
01445       throw;
01446     }
01447     
01448     /* Handle write buffer exceptions */
01449     catch (SickIOException &sick_io_exception) {
01450       std::cerr << sick_io_exception.what() << std::endl;
01451       throw;
01452     }
01453     
01454     /* A safety net */
01455     catch (...) {
01456       std::cerr << "SickLMS1xx::_startMeasuring: Unknown exception!!!" << std::endl;
01457       throw;
01458     }
01459 
01460     /* Reset the buffer (not necessary, but its better to do so just in case) */
01461     memset(payload_buffer,0,16);
01462 
01463     /* Extract the message payload */
01464     recv_message.GetPayload(payload_buffer);
01465     
01466     /* Check if it worked... */
01467     if (payload_buffer[17] != '0') {
01468         throw SickConfigException("SickLMS1xx::_startMeasuring: Unable to start measuring!");         
01469     }
01470     
01471   }
01472 
01476   void SickLMS1xx::_stopMeasuring( ) throw( SickTimeoutException, SickIOException ) {
01477 
01478     /* Allocate a single buffer for payload contents */
01479     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01480     
01481     /* Set the command type */
01482     payload_buffer[0]  = 's';
01483     payload_buffer[1]  = 'M';
01484     payload_buffer[2]  = 'N';
01485     payload_buffer[3]  = ' ';
01486     
01487     /* Set the command */
01488     payload_buffer[4]  = 'L';
01489     payload_buffer[5]  = 'M';
01490     payload_buffer[6]  = 'C';
01491     payload_buffer[7]  = 's';
01492     payload_buffer[8]  = 't';
01493     payload_buffer[9]  = 'o';
01494     payload_buffer[10] = 'p';
01495     payload_buffer[11] = 'm';
01496     payload_buffer[12] = 'e';
01497     payload_buffer[13] = 'a';
01498     payload_buffer[14] = 's';    
01499     
01500     /* Construct command message */
01501     SickLMS1xxMessage send_message(payload_buffer,15);
01502     
01503     /* Setup container for recv message */
01504     SickLMS1xxMessage recv_message;
01505     
01506     try {
01507       
01508       /* Send message and get reply */      
01509       _sendMessageAndGetReply(send_message, recv_message, "sAN", "LMCstopmeas");
01510       
01511     }
01512     
01513     /* Handle a timeout! */
01514     catch (SickTimeoutException &sick_timeout_exception) {
01515       std::cerr << sick_timeout_exception.what() << std::endl;
01516       throw;
01517     }
01518     
01519     /* Handle write buffer exceptions */
01520     catch (SickIOException &sick_io_exception) {
01521       std::cerr << sick_io_exception.what() << std::endl;
01522       throw;
01523     }
01524     
01525     /* A safety net */
01526     catch (...) {
01527       std::cerr << "SickLMS1xx::_stopMeasuring: Unknown exception!!!" << std::endl;
01528       throw;
01529     }
01530     
01531     /* Reset the buffer (not necessary, but its better to do so just in case) */
01532     memset(payload_buffer,0,15);
01533     
01534     /* Extract the message payload */
01535     recv_message.GetPayload(payload_buffer);
01536     
01537     /* Check if it worked... */
01538     if (payload_buffer[16] != '0') {
01539       throw SickConfigException("SickLMS1xx::_stopMeasuring: Unable to start measuring!");            
01540     }
01541     
01542   }
01543 
01549   void SickLMS1xx::_requestDataStream( ) throw( SickTimeoutException, SickConfigException, SickIOException ) {
01550 
01551     std::cout << std::endl << "\tRequesting data stream..." << std::endl;
01552     
01553     try {
01554       
01555       /* Wait for device to be measuring */
01556       _checkForMeasuringStatus();
01557       
01558       /* Request the data stream... */
01559       _startStreamingMeasurements();
01560       
01561     }
01562     
01563     /* Handle config exceptions */
01564     catch (SickConfigException &sick_config_exception) {
01565       std::cerr << sick_config_exception.what() << std::endl;
01566       throw;
01567     }
01568     
01569     /* Handle a timeout! */
01570     catch (SickTimeoutException &sick_timeout_exception) {
01571       std::cerr << sick_timeout_exception.what() << std::endl;
01572       throw;
01573     }
01574     
01575     /* Handle write buffer exceptions */
01576     catch (SickIOException &sick_io_exception) {
01577       std::cerr << sick_io_exception.what() << std::endl;
01578       throw;
01579     }
01580     
01581     catch (...) {
01582       std::cerr << "SickLMS1xx::SetSickScanArea: Unknown exception!!!" << std::endl;
01583       throw;
01584     }
01585 
01586     std::cout << "\t\tStream started!" << std::endl;
01587 
01588   }
01589   
01590   /*
01591    * \brief Start Streaming Values
01592    */
01593   void SickLMS1xx::_startStreamingMeasurements( ) throw( SickTimeoutException, SickIOException ) {
01594 
01595     /* Allocate a single buffer for payload contents */
01596     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01597     
01598     /* Set the command type */
01599     payload_buffer[0]  = 's';
01600     payload_buffer[1]  = 'E';
01601     payload_buffer[2]  = 'N';
01602     payload_buffer[3]  = ' ';
01603     
01604     /* Set the command */
01605     payload_buffer[4]  = 'L';
01606     payload_buffer[5]  = 'M';
01607     payload_buffer[6]  = 'D';
01608     payload_buffer[7]  = 's';
01609     payload_buffer[8]  = 'c';
01610     payload_buffer[9]  = 'a';
01611     payload_buffer[10] = 'n';
01612     payload_buffer[11] = 'd';
01613     payload_buffer[12] = 'a';
01614     payload_buffer[13] = 't';
01615     payload_buffer[14] = 'a';
01616     payload_buffer[15] = ' ';
01617 
01618     /* Start streaming! */
01619     payload_buffer[16] = '1';
01620     
01621     /* Construct command message */
01622     SickLMS1xxMessage send_message(payload_buffer,17);
01623 
01624     /* Setup container for recv message */
01625     SickLMS1xxMessage recv_message;
01626 
01627     try {
01628 
01629       /* Send message and get reply */      
01630       _sendMessageAndGetReply(send_message, recv_message, "sSN", "LMDscandata");
01631 
01632     }
01633         
01634     /* Handle a timeout! */
01635     catch (SickTimeoutException &sick_timeout_exception) {
01636       std::cerr << sick_timeout_exception.what() << std::endl;
01637       throw;
01638     }
01639     
01640     /* Handle write buffer exceptions */
01641     catch (SickIOException &sick_io_exception) {
01642       std::cerr << sick_io_exception.what() << std::endl;
01643       throw;
01644     }
01645     
01646     /* A safety net */
01647     catch (...) {
01648       std::cerr << "SickLMS1xx::_startStreamingMeasurements: Unknown exception!!!" << std::endl;
01649       throw;
01650     }
01651 
01652     /* Success! */
01653     _sick_streaming = true;
01654     
01655   }
01656 
01660   void SickLMS1xx::_stopStreamingMeasurements( const bool disp_banner ) throw( SickTimeoutException, SickIOException ) {
01661 
01662     if (disp_banner) {
01663       std::cout << "\tStopping data stream..." << std::endl;
01664     }
01665       
01666     /* Allocate a single buffer for payload contents */
01667     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01668     
01669     /* Set the command type */
01670     payload_buffer[0]  = 's';
01671     payload_buffer[1]  = 'E';
01672     payload_buffer[2]  = 'N';
01673     payload_buffer[3]  = ' ';
01674     
01675     /* Set the command */
01676     payload_buffer[4]  = 'L';
01677     payload_buffer[5]  = 'M';
01678     payload_buffer[6]  = 'D';
01679     payload_buffer[7]  = 's';
01680     payload_buffer[8]  = 'c';
01681     payload_buffer[9]  = 'a';
01682     payload_buffer[10] = 'n';
01683     payload_buffer[11] = 'd';
01684     payload_buffer[12] = 'a';
01685     payload_buffer[13] = 't';
01686     payload_buffer[14] = 'a';
01687     payload_buffer[15] = ' ';
01688 
01689     /* Start streaming! */
01690     payload_buffer[16] = '0';
01691     
01692     /* Construct command message */
01693     SickLMS1xxMessage send_message(payload_buffer,17);
01694 
01695     /* Setup container for recv message */
01696     SickLMS1xxMessage recv_message;
01697 
01698     try {
01699 
01700       /* Send message and get reply */      
01701       _sendMessage(send_message);
01702 
01703     }
01704         
01705     /* Handle write buffer exceptions */
01706     catch (SickIOException &sick_io_exception) {
01707       std::cerr << sick_io_exception.what() << std::endl;
01708       throw;
01709     }
01710     
01711     /* A safety net */
01712     catch (...) {
01713       std::cerr << "SickLMS1xx::_stopStreamingMeasurements: Unknown exception!!!" << std::endl;
01714       throw;
01715     }
01716 
01717     /* Success! */
01718     if (disp_banner) {
01719       std::cout << "\t\tStream stopped!" << std::endl;
01720     }
01721     
01722     _sick_streaming = false;
01723     
01724   }
01725 
01730   void SickLMS1xx::_checkForMeasuringStatus( unsigned int timeout_value ) throw( SickTimeoutException, SickIOException ) {
01731 
01732     /* Timeval structs for handling timeouts */
01733     struct timeval beg_time, end_time;
01734 
01735     /* Acquire the elapsed time since epoch */
01736     gettimeofday(&beg_time,NULL);
01737     
01738     /* Get device status */
01739     _updateSickStatus( );
01740 
01741     /* Check the shared object */
01742     bool first_pass = true;
01743     while( _sick_device_status != SICK_LMS_1XX_STATUS_READY_FOR_MEASUREMENT ) {    
01744 
01745       if (first_pass) {
01746 
01747         try {
01748           
01749           /* Tell the device to start measuring ! */
01750           _startMeasuring();
01751           first_pass = false;
01752           
01753         }
01754         
01755         /* Handle a timeout! */
01756         catch (SickTimeoutException &sick_timeout_exception) {
01757           std::cerr << sick_timeout_exception.what() << std::endl;
01758           throw;
01759         }
01760         
01761         /* Handle write buffer exceptions */
01762         catch (SickIOException &sick_io_exception) {
01763           std::cerr << sick_io_exception.what() << std::endl;
01764           throw;
01765         }
01766         
01767         /* A safety net */
01768         catch (...) {
01769           std::cerr << "SickLMS1xx::_checkForMeasuringStatus: Unknown exception!!!" << std::endl;
01770           throw;
01771         }
01772         
01773       }
01774 
01775       /* Sleep a little bit */
01776       usleep(1000);
01777       
01778       /* Check whether the allowed time has expired */
01779       gettimeofday(&end_time,NULL);    
01780       if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
01781         throw SickTimeoutException("SickLMS1xx::_checkForMeasuringStatus: Timeout occurred!");
01782       }
01783 
01784       /* Grab the latest device status */
01785       _updateSickStatus();
01786 
01787     }
01788 
01789   }
01790 
01794   void SickLMS1xx::_setSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException ) {
01795     
01796     /* Allocate a single buffer for payload contents */
01797     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01798 
01799     /* Set the command type */
01800     payload_buffer[0]  = 's';
01801     payload_buffer[1]  = 'W';
01802     payload_buffer[2]  = 'N';
01803     payload_buffer[3]  = ' ';
01804     
01805     /* Set the command */
01806     payload_buffer[4]  = 'L';
01807     payload_buffer[5]  = 'M';
01808     payload_buffer[6]  = 'D';
01809     payload_buffer[7]  = 's';
01810     payload_buffer[8]  = 'c';
01811     payload_buffer[9]  = 'a';
01812     payload_buffer[10] = 'n';
01813     payload_buffer[11] = 'd';
01814     payload_buffer[12] = 'a';
01815     payload_buffer[13] = 't';
01816     payload_buffer[14] = 'a';
01817     payload_buffer[15] = 'c';
01818     payload_buffer[16] = 'f';
01819     payload_buffer[17] = 'g';
01820     payload_buffer[18] = ' ';
01821 
01822     /* Specify the channel */
01823     payload_buffer[19] = '0'; 
01824 
01825     if (scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_NONE ||
01826         scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_8BIT ||
01827         scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_16BIT) {
01828       payload_buffer[20] = '1';
01829     }
01830     else {
01831       payload_buffer[20] = '3';
01832     }
01833     
01834     payload_buffer[21] = ' ';
01835 
01836     /* Values should be 0 */
01837     payload_buffer[22] = '0'; 
01838     payload_buffer[23] = '0'; 
01839     payload_buffer[24] = ' ';
01840 
01841     /* Send remission values? */
01842     if (scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_NONE ||
01843         scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_NONE) {
01844       payload_buffer[25] = '0';   // 0 = no, 1 = yes
01845     }
01846     else {
01847       payload_buffer[25] = '1';   // 0 = no, 1 = yes
01848     }
01849     payload_buffer[26] = ' ';
01850     
01851     /* Remission resolution */
01852     if (scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_16BIT ||
01853         scan_format == SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT) {
01854       payload_buffer[27] = '1';   // 0 = 8bit, 1 = 16bit
01855     }
01856     else {
01857       payload_buffer[27] = '0';   // 0 = 8bit, 1 = 16bit
01858     }
01859     payload_buffer[28] = ' ';
01860     
01861     /* Units (always 0) */
01862     payload_buffer[29] = '0';
01863     payload_buffer[30] = ' ';
01864     
01865     /* Encoder data? */
01866     payload_buffer[31] = '0'; // (00 = no encode data, 01 = channel 1 encoder)
01867     payload_buffer[32] = '0';
01868     payload_buffer[33] = ' ';
01869 
01870     /* These values should be 0 */
01871     payload_buffer[34] = '0';
01872     payload_buffer[35] = '0';
01873     payload_buffer[36] = ' ';
01874 
01875     /* Send position values? */
01876     payload_buffer[37] = '0';  // (0 = no position, 1 = send position)
01877     payload_buffer[38] = ' ';
01878 
01879     /* Send device name? */
01880     payload_buffer[39] = '0';  // (0 = no, 1 = yes)
01881     payload_buffer[40] = ' ';
01882 
01883     /* Send comment? */
01884     payload_buffer[41] = '0';  // (0 = no, 1 = yes)
01885     payload_buffer[42] = ' ';
01886 
01887     /* Send time info? */
01888     payload_buffer[43] = '0';  // (0 = no, 1 = yes)
01889     payload_buffer[44] = ' ';
01890 
01891     /* Send time info? */
01892     payload_buffer[45] = '+';  // +1 = send all scans, +2 every second scan, etc
01893     payload_buffer[46] = '1';
01894     
01895     /* Construct command message */
01896     SickLMS1xxMessage send_message(payload_buffer,47);
01897 
01898     /* Setup container for recv message */
01899     SickLMS1xxMessage recv_message;
01900 
01901     try {
01902 
01903       /* Send message and get reply */      
01904       _sendMessageAndGetReply(send_message, recv_message, "sWA", "LMDscandatacfg");
01905 
01906       /* Reinitialize the Sick so it uses the requested format */
01907       _reinitialize();
01908 
01909       /* Set the sick scan data format */
01910       _sick_scan_format = scan_format;
01911       
01912     }
01913         
01914     /* Handle a timeout! */
01915     catch (SickTimeoutException &sick_timeout_exception) {
01916       std::cerr << sick_timeout_exception.what() << std::endl;
01917       throw;
01918     }
01919     
01920     /* Handle write buffer exceptions */
01921     catch (SickIOException &sick_io_exception) {
01922       std::cerr << sick_io_exception.what() << std::endl;
01923       throw;
01924     }
01925 
01926     /* Handle thread exception */
01927     catch (SickThreadException &sick_thread_exception) {
01928       std::cerr << sick_thread_exception.what() << std::endl;
01929       throw;
01930     }
01931 
01932     /* Handle Sick error */
01933     catch (SickErrorException &sick_error_exception) {
01934       std::cerr << sick_error_exception.what() << std::endl;
01935       throw;
01936     }
01937     
01938     catch (...) {
01939       std::cerr << "SickLMS1xx::_setSickScanDataFormat: Unknown exception!!!" << std::endl;
01940       throw;
01941     }
01942 
01943     /* Success! */
01944     
01945   }
01946   
01950   void SickLMS1xx::_restoreMeasuringMode( ) throw( SickTimeoutException, SickIOException ) {
01951 
01952      /* Allocate a single buffer for payload contents */
01953     uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01954 
01955     /* Set the command type */
01956     payload_buffer[0]  = 's';
01957     payload_buffer[1]  = 'M';
01958     payload_buffer[2]  = 'N';
01959     payload_buffer[3]  = ' ';
01960     
01961     /* Set the command */
01962     payload_buffer[4]  = 'R';
01963     payload_buffer[5]  = 'u';
01964     payload_buffer[6]  = 'n';
01965     
01966     /* Construct command message */
01967     SickLMS1xxMessage send_message(payload_buffer,7);
01968 
01969     /* Setup container for recv message */
01970     SickLMS1xxMessage recv_message;
01971 
01972     try {
01973 
01974       /* Send message and get reply */      
01975       _sendMessageAndGetReply(send_message, recv_message, "sWA", "LMDscandatacfg");
01976 
01977     }
01978         
01979     /* Handle a timeout! */
01980     catch (SickTimeoutException &sick_timeout_exception) {
01981       std::cerr << sick_timeout_exception.what() << std::endl;
01982       throw;
01983     }
01984     
01985     /* Handle write buffer exceptions */
01986     catch (SickIOException &sick_io_exception) {
01987       std::cerr << sick_io_exception.what() << std::endl;
01988       throw;
01989     }
01990     
01991     /* A safety net */
01992     catch (...) {
01993       std::cerr << "SickLMS1xx::_restoreMeasuringMode: Unknown exception!!!" << std::endl;
01994       throw;
01995     }
01996 
01997     memset(payload_buffer,0,7);
01998     recv_message.GetPayload(payload_buffer);
01999     
02000     /* Check return value */
02001     if (payload_buffer[8] != '0') {
02002       std::cerr << "SickLMS1xx::_restoreMeasuringMode: Unknown exception!!!" << std::endl;
02003       throw;
02004     }
02005 
02006     /* Success! */
02007 
02008   }
02009   
02013   bool SickLMS1xx::_validScanArea( const int start_angle, const int stop_angle ) const {
02014 
02015     /* Ensure stop is greater than start */
02016     if (start_angle >= stop_angle) {
02017       return false;
02018     }
02019     
02020     /* Check angular bounds */
02021     if (start_angle < SICK_LMS_1XX_SCAN_AREA_MIN_ANGLE || start_angle > SICK_LMS_1XX_SCAN_AREA_MAX_ANGLE) {
02022       return false;
02023     }
02024 
02025     /* Check angular bounds */
02026     if (stop_angle < SICK_LMS_1XX_SCAN_AREA_MIN_ANGLE || stop_angle > SICK_LMS_1XX_SCAN_AREA_MAX_ANGLE) {
02027       return false;
02028     }
02029     
02030     /* Valid! */
02031     return true;
02032 
02033   }
02034 
02038   void SickLMS1xx::_sendMessage( const SickLMS1xxMessage &send_message ) const throw( SickIOException ) {
02039 
02040     try {
02041       
02042       /* Send a message using parent's method */
02043       SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage >::_sendMessage(send_message,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT);
02044       
02045     }
02046     
02047     /* Handle write buffer exceptions */
02048     catch (SickIOException &sick_io_error) {
02049       std::cerr << sick_io_error.what() << std::endl;
02050       throw;
02051     }
02052     
02053     /* A safety net */
02054     catch (...) {
02055       std::cerr << "SickLMS1xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
02056       throw;
02057     }
02058     
02059     /* Success */
02060         
02061   }
02062 
02072   void SickLMS1xx::_sendMessageAndGetReply( const SickLMS1xxMessage &send_message,
02073                                             SickLMS1xxMessage &recv_message,
02074                                             const std::string reply_command_type,
02075                                             const std::string reply_command,
02076                                             const unsigned int timeout_value,
02077                                             const unsigned int num_tries ) throw( SickIOException, SickTimeoutException ) {
02078 
02079     /* Construct the expected string */
02080     std::string expected_str = reply_command_type + " " + reply_command;
02081     
02082     try {
02083 
02084       /* Send a message and get reply using parent's method */
02085       SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage >::_sendMessageAndGetReply(send_message,recv_message,(uint8_t *)expected_str.c_str(),expected_str.length(),DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT,timeout_value,num_tries);
02086 
02087     }
02088     
02089     /* Handle a timeout! */
02090     catch (SickTimeoutException &sick_timeout) {
02091       throw;
02092     }
02093     
02094     /* Handle write buffer exceptions */
02095     catch (SickIOException &sick_io_error) {
02096       std::cerr << sick_io_error.what() << std::endl;
02097       throw;
02098     }
02099     
02100     /* A safety net */
02101     catch (...) {
02102       std::cerr << "SickLMS1xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
02103       throw;
02104     }
02105 
02106     /* Success! */
02107     
02108   }
02109 
02113   void SickLMS1xx::_recvMessage( SickLMS1xxMessage &sick_message ) const throw ( SickTimeoutException ) {
02114 
02115     try {
02116     
02117       /* Receive message using parent's method */
02118       SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage >::_recvMessage(sick_message,DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT);
02119 
02120     }
02121 
02122     /* Handle a timeout! */
02123     catch (SickTimeoutException &sick_timeout) {
02124       throw;
02125     }
02126     
02127     /* A safety net */
02128     catch (...) {
02129       std::cerr << "SickLMS1xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
02130       throw;
02131     }
02132     
02133   }
02134 
02139   sick_lms_1xx_status_t SickLMS1xx::_intToSickStatus( const int status ) const
02140   {
02141     switch(status) {
02142     case 1:
02143       return SICK_LMS_1XX_STATUS_INITIALIZATION;
02144     case 2:
02145       return SICK_LMS_1XX_STATUS_CONFIGURATION;
02146     case 3:
02147       return SICK_LMS_1XX_STATUS_IDLE;
02148     case 4:
02149       return SICK_LMS_1XX_STATUS_ROTATED;
02150     case 5:
02151       return SICK_LMS_1XX_STATUS_IN_PREP;
02152     case 6:
02153       return SICK_LMS_1XX_STATUS_READY;
02154     case 7:
02155       return SICK_LMS_1XX_STATUS_READY_FOR_MEASUREMENT;
02156     default:
02157       return SICK_LMS_1XX_STATUS_UNKNOWN;
02158     }
02159   }
02160 
02162   std::string SickLMS1xx::_intToSickConfigErrorStr( const int error ) const {
02163 
02164     switch(error) {
02165     case 1:
02166       return "Invalid Scan Frequency";
02167     case 2:
02168       return "Invalid Scan Resolution";
02169     case 3:
02170       return "Invalid Scan Frequency and Scan Resolution";
02171     case 4:
02172       return "Invalid Scan Area";
02173     default:
02174       return "Other Error";
02175     }
02176   
02177   }
02178 
02182   void SickLMS1xx::_printSickScanConfig( ) const {
02183 
02184     std::cout << "\t========= Sick Scan Config =========" << std::endl;
02185     std::cout << "\tScan Frequency: " << ((double)_sick_scan_config.sick_scan_freq)/100 << "(Hz)" << std::endl;  
02186     std::cout << "\tScan Resolution: " << ((double)_sick_scan_config.sick_scan_res)/10000 << " (deg)" << std::endl;  
02187     std::cout << "\tScan Area: " << "[" << ((double)_sick_scan_config.sick_start_angle)/10000 << "," << ((double)_sick_scan_config.sick_stop_angle)/10000 << "]" << std::endl;
02188     std::cout << "\t====================================" << std::endl;
02189     std::cout << std::endl << std::flush;
02190   }
02191   
02195   void SickLMS1xx::_printInitFooter( ) const {
02196 
02197     std::cout << "\t*** Init. complete: Sick LMS 1xx is online and ready!" << std::endl; 
02198     std::cout << "\tScan Frequency: " << _convertSickFreqUnitsToHz(_sick_scan_config.sick_scan_freq) << "(Hz)" << std::endl;  
02199     std::cout << "\tScan Resolution: " << _convertSickAngleUnitsToDegs(_sick_scan_config.sick_scan_res) << " (deg)" << std::endl;
02200     std::cout << "\tScan Area: " <<  "[" << _convertSickAngleUnitsToDegs(_sick_scan_config.sick_start_angle) << "," << _convertSickAngleUnitsToDegs(_sick_scan_config.sick_stop_angle) << "]" << std::endl;
02201     std::cout << std::endl;
02202     
02203   }
02204 
02210   std::string SickLMS1xx::_sickScanDataFormatToString( const sick_lms_1xx_scan_format_t scan_format ) const {
02211     
02212     /* Determine the type of distance measurements */
02213     switch (scan_format) {
02214     case SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_NONE:
02215       return "(single-pulse dist, no reflect)";
02216     case SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_8BIT:
02217       return "(single-pulse dist, 8Bit reflect)";
02218     case SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_16BIT: 
02219       return "(single-pulse dist, 16Bit reflect)";
02220     case SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_NONE:
02221       return "(double-pulse dist, no reflect)";
02222     case SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_8BIT:
02223       return "(double-pulse dist, 8Bit reflect)";
02224     case SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT:            
02225       return "(double-pulse dist, 16Bit reflect)";
02226     default:
02227       return "Unknown";
02228     }
02229 
02230   } 
02231 
02242   bool SickLMS1xx::_findSubString( const char * const str, const char * const substr,
02243                                    const unsigned int str_length, const unsigned int substr_length,
02244                                    unsigned int &substr_pos, unsigned int start_pos ) const {
02245     
02246     /* Init substring position */
02247     substr_pos = 0;
02248     
02249     /* Look for the substring */
02250     bool substr_found = false;
02251     for (unsigned int i = start_pos; !substr_found && (i < (str_length - substr_length) + 1); i++) {
02252       
02253       unsigned int j = 0;
02254       for (unsigned int k = i; (str[k] == substr[j]) && (j < substr_length); k++, j++);
02255       
02256       if (j == substr_length) {
02257         substr_found = true;
02258         substr_pos = i;
02259       }
02260       
02261     }
02262     
02263     /* Found! */
02264     return substr_found;
02265     
02266   }
02267 
02275   char * SickLMS1xx::_convertNextTokenToUInt( char * const str_buffer, unsigned int & num_val,
02276                                               const char * const delimeter ) const {
02277 
02278     const char * token = NULL;
02279     uint32_t curr_val = 0;
02280     if ((token = strtok(str_buffer,delimeter)) == NULL) {
02281       throw SickIOException("SickLMS1xx::_getextTokenAsUInt: strtok() failed!");
02282     }
02283 
02284     if (sscanf(token,"%x",&curr_val) == EOF) {
02285       throw SickIOException("SickLMS1xx::_getNextTokenAsUInt: sscanf() failed!");
02286     }
02287 
02288     num_val = (unsigned int)sick_lms_1xx_to_host_byte_order(curr_val);
02289     
02290     return str_buffer + strlen(token) + 1;
02291     
02292   }
02293   
02294 } //namespace SickToolbox


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Sun May 5 2019 02:28:23