SickLMS2xx.cc
Go to the documentation of this file.
00001 
00016 /* Auto-generated header */
00017 #include <sicktoolbox/SickConfig.hh>
00018 
00019 /* Implementation dependencies */
00020 #include <sstream>
00021 #include <iostream>
00022 #include <termios.h>
00023 #include <sys/ioctl.h>
00024 #include <signal.h>
00025 
00026 #include <sicktoolbox/SickLMS2xx.hh>
00027 #include <sicktoolbox/SickLMS2xxMessage.hh>
00028 #include <sicktoolbox/SickLMS2xxBufferMonitor.hh>
00029 #include <sicktoolbox/SickLMS2xxUtility.hh>
00030 #include <sicktoolbox/SickException.hh>
00031 
00032 #ifdef HAVE_LINUX_SERIAL_H
00033 #include <linux/serial.h>
00034 #else
00035 #define B500000 0010005
00036 #endif
00037 
00038 /* Associate the namespace */
00039 namespace SickToolbox {
00040 
00045   SickLMS2xx::SickLMS2xx( const std::string sick_device_path ): SickLIDAR< SickLMS2xxBufferMonitor, SickLMS2xxMessage >( ),
00046                                                                 _sick_device_path(sick_device_path),
00047                                                                 _curr_session_baud(SICK_BAUD_UNKNOWN),
00048                                                                 _desired_session_baud(SICK_BAUD_UNKNOWN),
00049                                                                 _sick_type(SICK_LMS_TYPE_UNKNOWN),
00050                                                                 _sick_mean_value_sample_size(0),
00051                                                                 _sick_values_subrange_start_index(0),
00052                                                                 _sick_values_subrange_stop_index(0)
00053   {
00054     
00055     /* Initialize the protected/private structs */
00056     memset(&_sick_operating_status,0,sizeof(sick_lms_2xx_operating_status_t));
00057     memset(&_sick_software_status,0,sizeof(sick_lms_2xx_software_status_t));
00058     memset(&_sick_restart_status,0,sizeof(sick_lms_2xx_restart_status_t));
00059     memset(&_sick_pollution_status,0,sizeof(sick_lms_2xx_pollution_status_t));
00060     memset(&_sick_signal_status,0,sizeof(sick_lms_2xx_signal_status_t));
00061     memset(&_sick_field_status,0,sizeof(sick_lms_2xx_field_status_t));
00062     memset(&_sick_baud_status,0,sizeof(sick_lms_2xx_baud_status_t));
00063     memset(&_sick_device_config,0,sizeof(sick_lms_2xx_device_config_t));
00064     memset(&_old_term,0,sizeof(struct termios));
00065     
00066   }
00067 
00071   SickLMS2xx::~SickLMS2xx() {
00072 
00073     try {
00074 
00075       /* Attempt to uninitialize the device */
00076       _teardownConnection();
00077       
00078     }
00079 
00080     /* Catch an I/O exception */
00081     catch(SickIOException &sick_io_exception) {
00082       std::cerr << sick_io_exception.what() << std::endl;
00083     }
00084     
00085     /* Catch anything else */
00086     catch(...) {
00087       std::cerr << "SickLMS2xx::~SickLMS2xx: Unknown exception!" << std::endl;
00088     }
00089     
00090   }
00091   
00098   void SickLMS2xx::Initialize( const sick_lms_2xx_baud_t desired_baud_rate, const uint32_t delay )
00099     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00100 
00101     /* Buffer the desired baud rate in case we have to reset */
00102     _desired_session_baud = desired_baud_rate;
00103     
00104     try {
00105     
00106       std::cout << std::endl << "\t*** Attempting to initialize the Sick LMS..." << std::endl << std::flush;
00107       
00108       /* Initialize the serial term for communication */
00109       std::cout << "\tAttempting to open device @ " << _sick_device_path << std::endl << std::flush;
00110       _setupConnection(delay);
00111       std::cout << "\t\tDevice opened!" << std::endl << std::flush;
00112 
00113       /* Start/reset the buffer monitor */
00114       if (!_sick_monitor_running) {
00115         std::cout << "\tAttempting to start buffer monitor..." << std::endl;       
00116         _startListening();
00117         std::cout << "\t\tBuffer monitor started!" << std::endl;
00118       }
00119       else {
00120         std::cout << "\tAttempting to reset buffer monitor..." << std::endl;       
00121         _sick_buffer_monitor->SetDataStream(_sick_fd);
00122         std::cout << "\t\tBuffer monitor reset!" << std::endl;       
00123       }
00124 
00125       try {
00126 
00127         std::cout << "\tAttempting to set requested baud rate..." << std::endl;
00128         _setSessionBaud(_desired_session_baud);
00129         
00130       }
00131 
00132       /* Assume a timeout is due to a misconfigured terminal baud */
00133       catch(SickTimeoutException &sick_timeout) {
00134       
00135         /* Check whether to do an autodetect */
00136         sick_lms_2xx_baud_t default_baud = _baudToSickBaud(DEFAULT_SICK_LMS_2XX_SICK_BAUD);
00137         std::cout << "\tFailed to set requested baud rate..." << std::endl << std::flush;
00138         std::cout << "\tAttempting to detect LMS baud rate..." << std::endl << std::flush;
00139         if((default_baud != SICK_BAUD_9600) && _testSickBaud(SICK_BAUD_9600)) {
00140           std::cout << "\t\tDetected LMS baud @ " << SickBaudToString(SICK_BAUD_9600) << "!" << std::endl;
00141         } else if((default_baud != SICK_BAUD_19200) && _testSickBaud(SICK_BAUD_19200)) {
00142           std::cout << "\t\tDetected LMS baud @ " << SickBaudToString(SICK_BAUD_19200) << "!" << std::endl;
00143         } else if((default_baud != SICK_BAUD_38400) && _testSickBaud(SICK_BAUD_38400)) {
00144           std::cout << "\t\tDetected LMS baud @ " << SickBaudToString(SICK_BAUD_38400) << "!" << std::endl;
00145         } else if((default_baud) != SICK_BAUD_500K && _testSickBaud(SICK_BAUD_500K)) {
00146           std::cout << "\t\tDetected LMS baud @ " << SickBaudToString(SICK_BAUD_500K) << "!" << std::endl;
00147         } else {
00148           _stopListening();
00149           throw SickIOException("SickLMS2xx::Initialize: failed to detect baud rate!"); 
00150         }
00151         std::cout << std::flush;
00152         
00153         /* Try again! */
00154         if (_curr_session_baud != _desired_session_baud) {
00155           std::cout << "\tAttempting to setup desired baud (again)..." << std::endl << std::flush;
00156           _setSessionBaud(_desired_session_baud);         
00157         }
00158         
00159       }
00160 
00161       /* Catch anything else */
00162       catch(...) {
00163         std::cerr << "SickLMS2xx::Initialize: Unknown exception!" << std::endl;
00164         throw;
00165       }
00166 
00167       std::cout << "\t\tOperating @ " << SickBaudToString(_curr_session_baud) << std::endl;     
00168       
00169       /* Set the device to request range mode */
00170       _setSickOpModeMonitorRequestValues();
00171       
00172       /* Acquire the type of device that we are working with */
00173       std::cout << "\tAttempting to sync driver..." << std::endl << std::flush;
00174       _getSickType();     // Get the Sick device type string
00175       _getSickStatus();   // Get the Sick device status
00176       _getSickConfig();   // Get the Sick current config
00177       std::cout << "\t\tDriver synchronized!" << std::endl << std::flush;
00178 
00179       /* Set the flag */
00180       _sick_initialized = true;
00181       
00182     }
00183 
00184     /* Handle a config exception */
00185     catch(SickConfigException &sick_config_exception) {
00186       std::cerr << sick_config_exception.what() << std::endl;
00187       throw;
00188     }
00189     
00190     /* Handle a timeout exception */
00191     catch(SickTimeoutException &sick_timeout_exception) {
00192       std::cerr << sick_timeout_exception.what() << std::endl;
00193       throw;
00194     }
00195     
00196     /* Handle any I/O exceptions */
00197     catch(SickIOException &sick_io_exception) {
00198       std::cerr << sick_io_exception.what() << std::endl;
00199       throw;
00200     }
00201 
00202     /* Handle any thread exceptions */
00203     catch(SickThreadException &sick_thread_exception) {
00204       std::cerr << sick_thread_exception.what() << std::endl;
00205       throw;
00206     }
00207 
00208     /* Handle anything else */
00209     catch(...) {
00210       std::cerr << "SickLMS2xx::Initialize: Unknown exception!" << std::endl;
00211       throw;
00212     }
00213 
00214     /* Initialization was successful! */
00215     std::cout << "\t*** Init. complete: Sick LMS is online and ready!" << std::endl; 
00216     std::cout << "\tSick Type: " << SickTypeToString(GetSickType()) << std::endl;
00217     std::cout << "\tScan Angle: " << GetSickScanAngle() << " (deg)" << std::endl;  
00218     std::cout << "\tScan Resolution: " << GetSickScanResolution() << " (deg)" << std::endl;
00219     std::cout << "\tMeasuring Mode: " << SickMeasuringModeToString(GetSickMeasuringMode()) << std::endl;
00220     std::cout << "\tMeasuring Units: " << SickMeasuringUnitsToString(GetSickMeasuringUnits()) << std::endl;
00221     std::cout << std::endl << std::flush;
00222     
00223   }
00224 
00229   void SickLMS2xx::Uninitialize( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00230 
00231     if (_sick_initialized) {
00232 
00233       std::cout << std::endl << "\t*** Attempting to uninitialize the Sick LMS..." << std::endl;       
00234 
00235       try {
00236         
00237         /* Restore original operating mode */
00238         _setSickOpModeMonitorRequestValues();
00239         
00240         /* Restore original baud rate settings */
00241         _setSessionBaud(_baudToSickBaud(DEFAULT_SICK_LMS_2XX_SICK_BAUD));
00242 
00243         /* Attempt to cancel the buffer monitor */
00244         if (_sick_monitor_running) {
00245           std::cout << "\tAttempting to stop buffer monitor..." << std::endl;
00246           _stopListening();
00247           std::cout << "\t\tBuffer monitor stopped!" << std::endl;
00248         }
00249         
00250         std::cout << "\t*** Uninit. complete - Sick LMS is now offline!" << std::endl << std::flush;
00251         
00252       }
00253     
00254       /* Handle any config exceptions */
00255       catch(SickConfigException &sick_config_exception) {
00256         std::cerr << sick_config_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00257         throw;
00258       }
00259       
00260       /* Handle a timeout exception */
00261       catch(SickTimeoutException &sick_timeout_exception) {
00262         std::cerr << sick_timeout_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00263         throw;
00264       }
00265       
00266       /* Handle any I/O exceptions */
00267       catch(SickIOException &sick_io_exception) {
00268         std::cerr << sick_io_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00269         throw;
00270       }
00271       
00272       /* Handle any thread exceptions */
00273       catch(SickThreadException &sick_thread_exception) {
00274         std::cerr << sick_thread_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00275         throw;
00276       }
00277       
00278       /* Handle anything else */
00279       catch(...) {
00280         std::cerr << "SickLMS2xx::Unintialize: Unknown exception!!!" << std::endl;
00281         throw;
00282       }
00283 
00284       /* Reset the flag */
00285       _sick_initialized = false;
00286       
00287     }
00288       
00289   }
00290 
00295   std::string SickLMS2xx::GetSickDevicePath( ) const {
00296     return _sick_device_path;
00297   }
00298   
00303   sick_lms_2xx_type_t SickLMS2xx::GetSickType( ) const throw( SickConfigException ) {
00304 
00305     /* Ensure the device is initialized */
00306     if (!_sick_initialized) {
00307       throw SickConfigException("SickLMS2xx::GetSickType: Sick LMS is not initialized!");
00308     }
00309 
00310     /* Return the Sick LMS type */
00311     return _sick_type;
00312     
00313   }
00314 
00319   double SickLMS2xx::GetSickScanAngle( ) const throw( SickConfigException ) {
00320 
00321     /* Ensure the device is initialized */
00322     if (!_sick_initialized) {
00323       throw SickConfigException("SickLMS2xx::GetSickScanAngle: Sick LMS is not initialized!");
00324     }
00325 
00326     /* Return the Sick scan angle */
00327     return (double)_sick_operating_status.sick_scan_angle;
00328 
00329   }
00330 
00335   double SickLMS2xx::GetSickScanResolution( ) const throw( SickConfigException ) {
00336 
00337     /* Ensure the device is initialized */
00338     if (!_sick_initialized) {
00339       throw SickConfigException("SickLMS2xx::GetSickScanResolution: Sick LMS is not initialized!");
00340     }
00341 
00342     /* Return the scan resolution */
00343     return _sick_operating_status.sick_scan_resolution*(0.01);
00344 
00345   }
00346     
00351   void SickLMS2xx::SetSickMeasuringUnits( const sick_lms_2xx_measuring_units_t sick_units )
00352     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00353 
00354     /* Ensure the device is initialized */
00355     if (!_sick_initialized) {
00356       throw SickConfigException("SickLMS2xx::SetSickMeasuringUnits: Sick LMS is not initialized!");
00357     }
00358 
00359     /* Ensure a valid units type was given */
00360     if (!_validSickMeasuringUnits(sick_units)) {
00361       throw SickConfigException("SickLMS2xx::SetSickMeasuringUnits: Undefined measurement units!");
00362     }
00363 
00364     /* Make sure the write is necessary */
00365     if (sick_units != _sick_device_config.sick_measuring_units) {
00366       
00367       /* Setup a local copy of the config */
00368       sick_lms_2xx_device_config_t sick_device_config;
00369       
00370       /* Copy the configuration locally */
00371       sick_device_config = _sick_device_config;
00372 
00373       /* Set the units value */
00374       sick_device_config.sick_measuring_units = sick_units;
00375 
00376       try {
00377         
00378         /* Attempt to set the new configuration */
00379         _setSickConfig(sick_device_config);
00380         
00381       }
00382         
00383       /* Handle any config exceptions */
00384       catch(SickConfigException &sick_config_exception) {
00385         std::cerr << sick_config_exception.what() << std::endl;
00386         throw;
00387       }
00388       
00389       /* Handle a timeout exception */
00390       catch(SickTimeoutException &sick_timeout_exception) {
00391         std::cerr << sick_timeout_exception.what() << std::endl;
00392         throw;
00393       }
00394       
00395       /* Handle any I/O exceptions */
00396       catch(SickIOException &sick_io_exception) {
00397         std::cerr << sick_io_exception.what() << std::endl;
00398         throw;
00399       }
00400       
00401       /* Handle any thread exceptions */
00402       catch(SickThreadException &sick_thread_exception) {
00403         std::cerr << sick_thread_exception.what() << std::endl;
00404         throw;
00405       }
00406       
00407       /* Handle anything else */
00408       catch(...) {
00409         std::cerr << "SickLMS2xx::SetSickMeasuringUnits: Unknown exception!!!" << std::endl;
00410         throw;
00411       }
00412 
00413     }
00414     else {
00415       std::cerr << "\tSickLMS2xx::SetSickMeasuringUnits - Device is already configured w/ these units. (skipping write)" << std::endl;
00416     }
00417       
00418   }
00419 
00424   sick_lms_2xx_measuring_units_t SickLMS2xx::GetSickMeasuringUnits( ) const throw( SickConfigException ) {
00425 
00426     /* Ensure the device is initialized */
00427     if (!_sick_initialized) {
00428       throw SickConfigException("SickLMS2xx::GetSickMeasuringUnits: Sick LMS is not initialized!");
00429     }
00430 
00431     /* Return the measurement units */
00432     return (sick_lms_2xx_measuring_units_t)_sick_operating_status.sick_measuring_units;
00433 
00434   }
00435   
00440   void SickLMS2xx::SetSickSensitivity( const sick_lms_2xx_sensitivity_t sick_sensitivity )
00441     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00442 
00443     /* Ensure the device is initialized */
00444     if (!_sick_initialized) {
00445       throw SickConfigException("SickLMS2xx::SetSickSensitivity: Sick LMS is not initialized!");
00446     }
00447     
00448     /* Ensure the command is supported by the given Sick model */
00449     if (!_isSickLMS211() && !_isSickLMS221() && !_isSickLMS291()) {
00450       throw SickConfigException("SickLMS2xx::SetSickSensitivity: This command is not supported by this Sick model!");
00451     }
00452 
00453     /* Ensure this is a valid sick sensitivity value*/
00454     if (!_validSickSensitivity(sick_sensitivity)) {
00455       throw SickConfigException("SickLMS2xx::SetSickSensitivity: Undefined sensitivity level!");
00456     }
00457 
00458     /* Make sure the write is necessary */
00459     if (sick_sensitivity != _sick_device_config.sick_peak_threshold) {
00460       
00461       /* Setup a local copy of the config */
00462       sick_lms_2xx_device_config_t sick_device_config;
00463       
00464       /* Copy the configuration locally */
00465       sick_device_config = _sick_device_config;
00466 
00467       /* Set the units value */
00468       sick_device_config.sick_peak_threshold = sick_sensitivity;
00469 
00470       try {
00471         
00472         /* Attempt to set the new configuration */
00473         _setSickConfig(sick_device_config);
00474         
00475       }
00476         
00477       /* Handle any config exceptions */
00478       catch(SickConfigException &sick_config_exception) {
00479         std::cerr << sick_config_exception.what() << std::endl;
00480         throw;
00481       }
00482       
00483       /* Handle a timeout exception */
00484       catch(SickTimeoutException &sick_timeout_exception) {
00485         std::cerr << sick_timeout_exception.what() << std::endl;
00486         throw;
00487       }
00488       
00489       /* Handle any I/O exceptions */
00490       catch(SickIOException &sick_io_exception) {
00491         std::cerr << sick_io_exception.what() << std::endl;
00492         throw;
00493       }
00494       
00495       /* Handle any thread exceptions */
00496       catch(SickThreadException &sick_thread_exception) {
00497         std::cerr << sick_thread_exception.what() << std::endl;
00498         throw;
00499       }
00500       
00501       /* Handle anything else */
00502       catch(...) {
00503         std::cerr << "SickLMS2xx::SetSickSensitivity: Unknown exception!!!" << std::endl;
00504         throw;
00505       }
00506 
00507     }
00508     else {
00509       std::cerr << "\tSickLMS2xx::SetSickSensitivity - Sick is already operating at this sensitivity level! (skipping write)" << std::endl;
00510     }
00511       
00512   }
00513 
00518   void SickLMS2xx::SetSickPeakThreshold( const sick_lms_2xx_peak_threshold_t sick_peak_threshold )
00519     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00520 
00521     /* Ensure the device is initialized */
00522     if (!_sick_initialized) {
00523       throw SickConfigException("SickLMS2xx::SetSickPeakThreshold: Sick LMS is not initialized!");
00524     }
00525     
00526     /* Ensure the command is supported by the given Sick model */
00527     if (!_isSickLMS200() && !_isSickLMS220()) {
00528       throw SickConfigException("SickLMS2xx::SetSickPeakThreshold: This command is not supported by this Sick model!");
00529     }
00530     
00531     /* Ensure this is a valid sick sensitivity value*/
00532     if (!_validSickPeakThreshold(sick_peak_threshold)) {
00533       throw SickConfigException("SickLMS2xx::SetSickPeakThreshold: Undefined peak threshold!");
00534     }
00535 
00536     /* Make sure the write is necessary */
00537     if (sick_peak_threshold != _sick_device_config.sick_peak_threshold) {
00538       
00539       /* Setup a local copy of the config */
00540       sick_lms_2xx_device_config_t sick_device_config;
00541       
00542       /* Copy the configuration locally */
00543       sick_device_config = _sick_device_config;
00544 
00545       /* Set the units value */
00546       sick_device_config.sick_peak_threshold = sick_peak_threshold;
00547 
00548       try {
00549         
00550         /* Attempt to set the new configuration */
00551         _setSickConfig(sick_device_config);
00552         
00553       }
00554         
00555       /* Handle any config exceptions */
00556       catch(SickConfigException &sick_config_exception) {
00557         std::cerr << sick_config_exception.what() << std::endl;
00558         throw;
00559       }
00560       
00561       /* Handle a timeout exception */
00562       catch(SickTimeoutException &sick_timeout_exception) {
00563         std::cerr << sick_timeout_exception.what() << std::endl;
00564         throw;
00565       }
00566       
00567       /* Handle any I/O exceptions */
00568       catch(SickIOException &sick_io_exception) {
00569         std::cerr << sick_io_exception.what() << std::endl;
00570         throw;
00571       }
00572       
00573       /* Handle any thread exceptions */
00574       catch(SickThreadException &sick_thread_exception) {
00575         std::cerr << sick_thread_exception.what() << std::endl;
00576         throw;
00577       }
00578       
00579       /* Handle anything else */
00580       catch(...) {
00581         std::cerr << "SickLMS2xx::SetSickPeakThreshold: Unknown exception!!!" << std::endl;
00582         throw;
00583       }
00584 
00585     }
00586     else {
00587       std::cerr << "\tSickLMS2xx::SetSickPeakThreshold - Sick is already operating w/ given threshold! (skipping write)" << std::endl;
00588     }
00589     
00590   }
00591   
00596   sick_lms_2xx_sensitivity_t SickLMS2xx::GetSickSensitivity( ) const throw( SickConfigException ) {
00597 
00598     /* Ensure the device is initialized */
00599     if (!_sick_initialized) {
00600       throw SickConfigException("SickLMS2xx::GetSickSensitivity: Sick LMS is not initialized!");
00601     }
00602     
00603     /* Make sure sensitivity is something that is defined for this model */
00604     if(!_isSickLMS211() && !_isSickLMS221() && !_isSickLMS291()) {
00605       std::cerr << "Sensitivity is undefined for model: " << SickTypeToString(GetSickType()) << " (returning \"Unknown\")" << std::endl;
00606       return SICK_SENSITIVITY_UNKNOWN;
00607     }
00608 
00609     /* If its supported than return the actual value */
00610     return (sick_lms_2xx_sensitivity_t)_sick_device_config.sick_peak_threshold;  //If the device is 211/221/291 then this value is sensitivity
00611 
00612   }
00613 
00618   sick_lms_2xx_peak_threshold_t SickLMS2xx::GetSickPeakThreshold( ) const throw( SickConfigException ) {
00619 
00620     /* Ensure the device is initialized */
00621     if (!_sick_initialized) {
00622       throw SickConfigException("SickLMS2xx::GetSickPeakThreshold: Sick LMS is not initialized!");
00623     }
00624     
00625     /* Make sure sensitivity is something that is defined for this model */
00626     if(!_isSickLMS200() && !_isSickLMS220()) {
00627       std::cerr << "Peak threshold is undefined for model: " << SickTypeToString(GetSickType()) << " (returning \"Unknown\")" << std::endl;
00628       return SICK_PEAK_THRESHOLD_UNKNOWN;
00629     }
00630 
00631     /* If its supported than return the actual value */
00632     return (sick_lms_2xx_peak_threshold_t)_sick_device_config.sick_peak_threshold;  //If the device is 211/221/291 then this value is sensitivity
00633 
00634   }
00635   
00640   void SickLMS2xx::SetSickMeasuringMode( const sick_lms_2xx_measuring_mode_t sick_measuring_mode )
00641     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00642 
00643     /* Ensure the device is initialized */
00644     if (!_sick_initialized) {
00645       throw SickConfigException("SickLMS2xx::SetSickMeasuringUnits: Sick LMS is not initialized!");
00646     }
00647     
00648     /* Ensure this is a valid sick sensitivity value*/
00649     if (!_validSickMeasuringMode(sick_measuring_mode)) {
00650       throw SickConfigException("SickLMS2xx::SetSickMeasuringMode: Undefined measuring mode!");
00651     }
00652 
00653     /* Make sure the write is necessary */
00654     if (sick_measuring_mode != _sick_device_config.sick_measuring_mode) {
00655       
00656       /* Setup a local copy of the config */
00657       sick_lms_2xx_device_config_t sick_device_config;
00658       
00659       /* Copy the configuration locally */
00660       sick_device_config = _sick_device_config;
00661 
00662       /* Set the units value */
00663       sick_device_config.sick_measuring_mode = sick_measuring_mode;
00664 
00665       try {
00666         
00667         /* Attempt to set the new configuration */
00668         _setSickConfig(sick_device_config);
00669         
00670       }
00671         
00672       /* Handle any config exceptions */
00673       catch(SickConfigException &sick_config_exception) {
00674         std::cerr << sick_config_exception.what() << std::endl;
00675         throw;
00676       }
00677       
00678       /* Handle a timeout exception */
00679       catch(SickTimeoutException &sick_timeout_exception) {
00680         std::cerr << sick_timeout_exception.what() << std::endl;
00681         throw;
00682       }
00683       
00684       /* Handle any I/O exceptions */
00685       catch(SickIOException &sick_io_exception) {
00686         std::cerr << sick_io_exception.what() << std::endl;
00687         throw;
00688       }
00689       
00690       /* Handle any thread exceptions */
00691       catch(SickThreadException &sick_thread_exception) {
00692         std::cerr << sick_thread_exception.what() << std::endl;
00693         throw;
00694       }
00695       
00696       /* Handle anything else */
00697       catch(...) {
00698         std::cerr << "SickLMS2xx::SetSickMeasuringMode: Unknown exception!!!" << std::endl;
00699         throw;
00700       }
00701 
00702     }
00703     else {
00704       std::cerr << "\tSickLMS2xx::SetSickMeasuringMode - Sick is already operating w/ this measuring mode! (skipping write)" << std::endl;
00705     }
00706       
00707   }
00708 
00713   sick_lms_2xx_measuring_mode_t SickLMS2xx::GetSickMeasuringMode( ) const throw( SickConfigException ) {
00714 
00715     /* Ensure the device is initialized */
00716     if (!_sick_initialized) {
00717       throw SickConfigException("SickLMS2xx::GetSickMeasuringMode: Sick LMS is not initialized!");
00718     }
00719 
00720     /* Return the determined measuring mode */
00721     return (sick_lms_2xx_measuring_mode_t)_sick_operating_status.sick_measuring_mode;
00722 
00723   }
00724 
00729   sick_lms_2xx_operating_mode_t SickLMS2xx::GetSickOperatingMode( ) const throw( SickConfigException ) {
00730 
00731     /* Ensure the device is initialized */
00732     if (!_sick_initialized) {
00733       throw SickConfigException("SickLMS2xx::GetSickScanAngle: Sick LMS is not initialized!");
00734     }
00735 
00736     /* Return the current operating mode of the device */
00737     return (sick_lms_2xx_operating_mode_t)_sick_operating_status.sick_operating_mode;
00738 
00739   }
00740   
00745   void SickLMS2xx::SetSickAvailability( const uint8_t sick_availability_flags )
00746     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00747 
00748     /* Ensure the device is initialized */
00749     if (!_sick_initialized) {
00750       throw SickConfigException("SickLMS2xx::SetSickAvailabilityFlags: Sick LMS is not initialized!");
00751     }
00752     
00753     /* Ensure this is a valid sick sensitivity value*/
00754     if ( sick_availability_flags > 7 ) {      
00755       throw SickConfigException("SickLMS2xx::SetSickAvailabilityFlags: Invalid availability!");
00756     }
00757 
00758     /* Setup a local copy of the config */
00759     sick_lms_2xx_device_config_t sick_device_config;
00760     
00761     /* Copy the configuration locally */
00762     sick_device_config = _sick_device_config;
00763     
00764     /* Maintain the higher level bits */
00765     sick_device_config.sick_availability_level &= 0xF8;
00766     
00767     /* Set the new availability flags */
00768     sick_device_config.sick_availability_level |= sick_availability_flags;
00769 
00770         /* Make sure the write is necessary */
00771     if (sick_device_config.sick_availability_level != _sick_device_config.sick_availability_level) {
00772       
00773       try {
00774         
00775         /* Attempt to set the new configuration */
00776         _setSickConfig(sick_device_config);
00777         
00778       }
00779         
00780       /* Handle any config exceptions */
00781       catch(SickConfigException &sick_config_exception) {
00782         std::cerr << sick_config_exception.what() << std::endl;
00783         throw;
00784       }
00785       
00786       /* Handle a timeout exception */
00787       catch(SickTimeoutException &sick_timeout_exception) {
00788         std::cerr << sick_timeout_exception.what() << std::endl;
00789         throw;
00790       }
00791       
00792       /* Handle any I/O exceptions */
00793       catch(SickIOException &sick_io_exception) {
00794         std::cerr << sick_io_exception.what() << std::endl;
00795         throw;
00796       }
00797       
00798       /* Handle any thread exceptions */
00799       catch(SickThreadException &sick_thread_exception) {
00800         std::cerr << sick_thread_exception.what() << std::endl;
00801         throw;
00802       }
00803       
00804       /* Handle anything else */
00805       catch(...) {
00806         std::cerr << "SickLMS2xx::SetSickAvailabilityFlags: Unknown exception!!!" << std::endl;
00807         throw;
00808       }
00809 
00810     }
00811     else {
00812       std::cerr << "\tSickLMS2xx::SetSickAvailability - Device is already operating w/ given availability. (skipping write)" << std::endl;
00813     }
00814     
00815   }
00816 
00821   uint8_t SickLMS2xx::GetSickAvailability( ) const throw( SickConfigException ) {
00822 
00823     /* Ensure the device is initialized */
00824     if (!_sick_initialized) {
00825       throw SickConfigException("SickLMS2xx::GetSickAvailabilityFlags: Sick LMS is not initialized!");
00826     }
00827 
00828     /* Return the availability flags */
00829     return _sick_device_config.sick_availability_level;
00830 
00831   }
00832   
00838   void SickLMS2xx::SetSickVariant( const sick_lms_2xx_scan_angle_t scan_angle, const sick_lms_2xx_scan_resolution_t scan_resolution )
00839     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
00840 
00841     /* Ensure the device is initialized */
00842     if (!_sick_initialized) {
00843       throw SickConfigException("SickLMS2xx::SetSickVariant: Sick LMS is not initialized!");
00844     }
00845     
00846     /* A sanity check to make sure that the command is supported */
00847     if (_sick_type == SICK_LMS_TYPE_211_S14 || _sick_type == SICK_LMS_TYPE_221_S14 ||_sick_type == SICK_LMS_TYPE_291_S14) {
00848       throw SickConfigException("SickLMS2xx::SetSickVariant: Command not supported on this model!");
00849     }
00850 
00851     /* Ensure the given scan angle is defined */
00852     if (!_validSickScanAngle(scan_angle)) {
00853       throw SickConfigException("SickLMS2xx::SetSickVariant: Undefined scan angle!");
00854     }
00855 
00856     /* Ensure the given scan resolution is defined */
00857     if (!_validSickScanResolution(scan_resolution)) {
00858       throw SickConfigException("SickLMS2xx::SetSickMeasuringUnits: Undefined scan resolution!");
00859     }
00860 
00861     SickLMS2xxMessage message, response;
00862     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00863     
00864     payload_buffer[0] = 0x3B; // Command to set sick variant
00865 
00866     /* Map the scan angle */
00867     switch(scan_angle) {
00868     case SICK_SCAN_ANGLE_100:
00869       payload_buffer[1] = 0x64;
00870       break;
00871     case SICK_SCAN_ANGLE_180:
00872       payload_buffer[1] = 0xB4;
00873       break;
00874     default:
00875       throw SickConfigException("SickLMS2xx::SetSickVariant: Given scan angle is invalid!");
00876     }
00877     
00878     /* Map the resolution */
00879     switch(scan_resolution) {
00880     case SICK_SCAN_RESOLUTION_100:
00881       payload_buffer[3] = 0x64;
00882       break;
00883     case SICK_SCAN_RESOLUTION_50:
00884       payload_buffer[3] = 0x32;
00885       break;
00886     case SICK_SCAN_RESOLUTION_25:
00887       payload_buffer[3] = 0x19;
00888       break;
00889     default:
00890       throw SickConfigException("SickLMS2xx::SetSickVariant: Given scan resolution is invalid!");
00891     }
00892     
00893     /* Build the request message */
00894     message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,5);
00895     
00896     try {
00897 
00898       /*
00899        * Set the device to request measured mode
00900        *
00901        * NOTE: This is done since the Sick stops sending
00902        *       data if the variant is reset  midstream.
00903        */
00904       _setSickOpModeMonitorRequestValues();
00905       
00906       /* Send the variant request and get a reply */
00907       _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
00908       
00909     }
00910 
00911     /* Handle a config exception */
00912     catch(SickConfigException &sick_config_exception) {
00913       std::cerr << sick_config_exception.what() << std::endl;
00914       throw;
00915     }
00916     
00917     /* Handle a timeout exception */
00918     catch(SickTimeoutException &sick_timeout_exception) {
00919       std::cerr << sick_timeout_exception.what() << std::endl;
00920       throw;
00921     }
00922 
00923     /* Handle any I/O exceptions */
00924     catch(SickIOException &sick_io_exception) {
00925       std::cerr << sick_io_exception.what() << std::endl;
00926       throw;
00927     }
00928 
00929     /* Handle any thread exceptions */
00930     catch(SickThreadException &sick_thread_exception) {
00931       std::cerr << sick_thread_exception.what() << std::endl;
00932       throw;
00933     }
00934 
00935     /* Handle anything else */
00936     catch(...) {
00937       std::cerr << "SickLMS2xx::SetSickVariant: Unknown exception!!!" << std::endl;
00938       throw;
00939     }
00940     
00941     /* Extract the payload length */
00942     response.GetPayload(payload_buffer);
00943 
00944     /* Check if the configuration was successful */
00945     if(payload_buffer[1] != 0x01) {
00946       throw SickConfigException("SickLMS2xx::SetSickVariant: Configuration was unsuccessful!");
00947     }
00948 
00949     /* Update the scan angle of the device */
00950     memcpy(&_sick_operating_status.sick_scan_angle,&payload_buffer[2],2);
00951     _sick_operating_status.sick_scan_angle =
00952       sick_lms_2xx_to_host_byte_order(_sick_operating_status.sick_scan_angle);
00953     
00954     /* Update the angular resolution of the device */
00955     memcpy(&_sick_operating_status.sick_scan_resolution,&payload_buffer[4],2);
00956     _sick_operating_status.sick_scan_resolution =
00957       sick_lms_2xx_to_host_byte_order(_sick_operating_status.sick_scan_resolution);
00958     
00959   }
00960 
00977   void SickLMS2xx::GetSickScan( unsigned int * const measurement_values,
00978                              unsigned int & num_measurement_values,
00979                              unsigned int * const sick_field_a_values,
00980                              unsigned int * const sick_field_b_values,
00981                              unsigned int * const sick_field_c_values,
00982                              unsigned int * const sick_telegram_index,
00983                              unsigned int * const sick_real_time_scan_index ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException) {
00984 
00985     /* Ensure the device is initialized */
00986     if (!_sick_initialized) {
00987       throw SickConfigException("SickLMS2xx::GetSickScan: Sick LMS is not initialized!");
00988     }
00989     
00990     /* Declare message objects */
00991     SickLMS2xxMessage response;
00992 
00993     /* Declare some useful variables and a buffer */
00994     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00995     
00996     try {
00997     
00998       /* Restore original operating mode */
00999       _setSickOpModeMonitorStreamValues();
01000       
01001       /* Receive a data frame from the stream. */
01002       _recvMessage(response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT);
01003       
01004       /* Check that our payload has the proper command byte of 0xB0 */
01005       if(response.GetCommandCode() != 0xB0) {
01006         throw SickIOException("SickLMS2xx::GetSickScan: Unexpected message!");
01007       }
01008 
01009       /* Acquire the payload buffer and length*/
01010       response.GetPayload(payload_buffer);
01011 
01012       /* Define a local scan profile object */
01013       sick_lms_2xx_scan_profile_b0_t sick_scan_profile;
01014 
01015       /* Initialize the profile */
01016       memset(&sick_scan_profile,0,sizeof(sick_lms_2xx_scan_profile_b0_t));
01017 
01018       /* Parse the message payload */
01019       _parseSickScanProfileB0(&payload_buffer[1],sick_scan_profile);
01020 
01021       /* Return the request values! */
01022       num_measurement_values = sick_scan_profile.sick_num_measurements;
01023 
01024       for (unsigned int i = 0; i < num_measurement_values; i++) {
01025 
01026         /* Copy the measurement value */
01027         measurement_values[i] = sick_scan_profile.sick_measurements[i];
01028 
01029         /* If requested, copy field A values */
01030         if(sick_field_a_values) {
01031           sick_field_a_values[i] = sick_scan_profile.sick_field_a_values[i];
01032         }
01033 
01034         /* If requested, copy field B values */
01035         if(sick_field_b_values) {
01036           sick_field_b_values[i] = sick_scan_profile.sick_field_b_values[i];
01037         }
01038 
01039         /* If requested, copy field C values */
01040         if(sick_field_c_values) {
01041           sick_field_c_values[i] = sick_scan_profile.sick_field_c_values[i];
01042         }
01043 
01044       }
01045 
01046       /* If requested, copy the real time scan index */
01047       if(sick_real_time_scan_index) {
01048         *sick_real_time_scan_index = sick_scan_profile.sick_real_time_scan_index;
01049       }
01050       
01051       /* If requested, copy the telegram index */
01052       if(sick_telegram_index) {
01053         *sick_telegram_index = sick_scan_profile.sick_telegram_index;
01054       }
01055 
01056     }
01057 
01058     /* Handle any config exceptions */
01059     catch(SickConfigException &sick_config_exception) {
01060       std::cerr << sick_config_exception.what() << std::endl;
01061       throw;
01062     }
01063     
01064     /* Handle a timeout exception */
01065     catch(SickTimeoutException &sick_timeout_exception) {
01066       std::cerr << sick_timeout_exception.what() << std::endl;
01067       throw;
01068     }
01069     
01070     /* Handle any I/O exceptions */
01071     catch(SickIOException &sick_io_exception) {
01072       std::cerr << sick_io_exception.what() << std::endl;
01073       throw;
01074     }
01075 
01076     /* Handle any thread exceptions */
01077     catch(SickThreadException &sick_thread_exception) {
01078       std::cerr << sick_thread_exception.what() << std::endl;
01079       throw;
01080     }
01081     
01082     /* Handle anything else */
01083     catch(...) {
01084       std::cerr << "SickLMS2xx::GetSickScan: Unknown exception!!!" << std::endl;
01085       throw;
01086     }
01087 
01088   }
01089 
01107   void SickLMS2xx::GetSickScan( unsigned int * const range_values,
01108                              unsigned int * const reflect_values,
01109                              unsigned int & num_range_measurements,
01110                              unsigned int & num_reflect_measurements,
01111                              unsigned int * const sick_field_a_values,
01112                              unsigned int * const sick_field_b_values,
01113                              unsigned int * const sick_field_c_values,
01114                              unsigned int * const sick_telegram_index,
01115                              unsigned int * const sick_real_time_scan_index ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException) {
01116 
01117     /* Ensure the device is initialized */
01118     if (!_sick_initialized) {
01119       throw SickConfigException("SickLMS2xx::GetSickScan: Sick LMS is not initialized!");
01120     }
01121     
01122     /* Declare message objects */
01123     SickLMS2xxMessage response;
01124 
01125     /* Declare some useful variables and a buffer */
01126     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01127     
01128     try {
01129       
01130       /* Restore original operating mode */
01131       _setSickOpModeMonitorStreamRangeAndReflectivity();
01132       
01133       /* Receive a data frame from the stream. */
01134       _recvMessage(response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT);
01135 
01136       /* Check that our payload has the proper command byte of 0xB0 */
01137       if(response.GetCommandCode() != 0xC4) {
01138         throw SickIOException("SickLMS2xx::GetSickScan: Unexpected message!");
01139       }
01140       
01141       /* Acquire the payload buffer and length*/
01142       response.GetPayload(payload_buffer);
01143       
01144       /* Define a local scan profile object */
01145       sick_lms_2xx_scan_profile_c4_t sick_scan_profile;
01146 
01147       /* Initialize the profile */
01148       memset(&sick_scan_profile,0,sizeof(sick_lms_2xx_scan_profile_c4_t));
01149 
01150       /* Parse the message payload */
01151       _parseSickScanProfileC4(&payload_buffer[1],sick_scan_profile);
01152 
01153       /* Return the requested values! */
01154       num_range_measurements = sick_scan_profile.sick_num_range_measurements;
01155       num_reflect_measurements = sick_scan_profile.sick_num_reflect_measurements;
01156       
01157       for (unsigned int i = 0; i < sick_scan_profile.sick_num_range_measurements; i++) {
01158 
01159         /* Copy the measurement value */
01160         range_values[i] = sick_scan_profile.sick_range_measurements[i];
01161         
01162         /* If requested, copy field A values */
01163         if(sick_field_a_values) {
01164           sick_field_a_values[i] = sick_scan_profile.sick_field_a_values[i];
01165         }
01166 
01167         /* If requested, copy field B values */
01168         if(sick_field_b_values) {
01169           sick_field_b_values[i] = sick_scan_profile.sick_field_b_values[i];
01170         }
01171 
01172         /* If requested, copy field C values */
01173         if(sick_field_c_values) {
01174           sick_field_c_values[i] = sick_scan_profile.sick_field_c_values[i];
01175         }
01176 
01177       }
01178 
01179       /* Copy the reflectivity measurements */
01180       for( unsigned int i = 0; i < num_reflect_measurements; i++) {
01181         reflect_values[i] = sick_scan_profile.sick_reflect_measurements[i];
01182       }
01183       
01184       /* If requested, copy the telegram index */
01185       if(sick_telegram_index) {
01186         *sick_telegram_index = sick_scan_profile.sick_telegram_index;
01187       }
01188       
01189       /* If requested, copy the real time scan index */
01190       if(sick_real_time_scan_index) {
01191         *sick_real_time_scan_index = sick_scan_profile.sick_real_time_scan_index;
01192       }
01193       
01194     }
01195 
01196     /* Handle any config exceptions */
01197     catch(SickConfigException &sick_config_exception) {
01198       std::cerr << sick_config_exception.what() << std::endl;
01199       throw;
01200     }
01201     
01202     /* Handle a timeout exception */
01203     catch(SickTimeoutException &sick_timeout_exception) {
01204       std::cerr << sick_timeout_exception.what() << std::endl;
01205       throw;
01206     }
01207     
01208     /* Handle any I/O exceptions */
01209     catch(SickIOException &sick_io_exception) {
01210       std::cerr << sick_io_exception.what() << std::endl;
01211       throw;
01212     }
01213 
01214     /* Handle any thread exceptions */
01215     catch(SickThreadException &sick_thread_exception) {
01216       std::cerr << sick_thread_exception.what() << std::endl;
01217       throw;
01218     }
01219     
01220     /* Handle anything else */
01221     catch(...) {
01222       std::cerr << "SickLMS2xx::GetSickScan: Unknown exception!!!" << std::endl;
01223       throw;
01224     }
01225 
01226   }
01227   
01250   void SickLMS2xx::GetSickScanSubrange( const uint16_t sick_subrange_start_index,
01251                                      const uint16_t sick_subrange_stop_index,
01252                                      unsigned int * const measurement_values,
01253                                      unsigned int & num_measurement_values,
01254                                      unsigned int * const sick_field_a_values,
01255                                      unsigned int * const sick_field_b_values,
01256                                      unsigned int * const sick_field_c_values,
01257                                      unsigned int * const sick_telegram_index,
01258                                      unsigned int * const sick_real_time_scan_index ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException) {
01259 
01260     /* Ensure the device is initialized */
01261     if (!_sick_initialized) {
01262       throw SickConfigException("SickLMS2xx::GetSickScanSubrange: Sick LMS is not initialized!");
01263     }
01264     
01265     /* Declare message object */
01266     SickLMS2xxMessage response;
01267 
01268     /* Declare some useful variables and a buffer */
01269     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01270     
01271     try {
01272     
01273       /* Restore original operating mode */
01274       _setSickOpModeMonitorStreamValuesSubrange(sick_subrange_start_index,sick_subrange_stop_index);
01275       
01276       /* Receive a data frame from the stream. */
01277       _recvMessage(response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT);
01278       
01279       /* Check that our payload has the proper command byte of 0xB0 */
01280       if(response.GetCommandCode() != 0xB7) {
01281         throw SickIOException("SickLMS2xx::GetSickScanSubrange: Unexpected message!");
01282       }
01283 
01284       /* Acquire the payload buffer and length*/
01285       response.GetPayload(payload_buffer);
01286 
01287       /* Define a local scan profile object */
01288       sick_lms_2xx_scan_profile_b7_t sick_scan_profile;
01289 
01290       /* Initialize the profile */
01291       memset(&sick_scan_profile,0,sizeof(sick_lms_2xx_scan_profile_b7_t));
01292 
01293       /* Parse the message payload */
01294       _parseSickScanProfileB7(&payload_buffer[1],sick_scan_profile);
01295 
01296       /* Return the request values! */
01297       num_measurement_values = sick_scan_profile.sick_num_measurements;
01298 
01299       for (unsigned int i = 0; i < num_measurement_values; i++) {
01300 
01301         /* Copy the measurement value */
01302         measurement_values[i] = sick_scan_profile.sick_measurements[i];
01303 
01304         /* If requested, copy field A values */
01305         if(sick_field_a_values) {
01306           sick_field_a_values[i] = sick_scan_profile.sick_field_a_values[i];
01307         }
01308 
01309         /* If requested, copy field B values */
01310         if(sick_field_b_values) {
01311           sick_field_b_values[i] = sick_scan_profile.sick_field_b_values[i];
01312         }
01313 
01314         /* If requested, copy field C values */
01315         if(sick_field_c_values) {
01316           sick_field_c_values[i] = sick_scan_profile.sick_field_c_values[i];
01317         }
01318 
01319       }
01320 
01321       /* If requested, copy the real time scan index */
01322       if(sick_real_time_scan_index) {
01323         *sick_real_time_scan_index = sick_scan_profile.sick_real_time_scan_index;
01324       }
01325       
01326       /* If requested, copy the telegram index */
01327       if(sick_telegram_index) {
01328         *sick_telegram_index = sick_scan_profile.sick_telegram_index;
01329       }
01330 
01331     }
01332 
01333     /* Handle any config exceptions */
01334     catch(SickConfigException &sick_config_exception) {
01335       std::cerr << sick_config_exception.what() << std::endl;
01336       throw;
01337     }
01338     
01339     /* Handle a timeout exception */
01340     catch(SickTimeoutException &sick_timeout_exception) {
01341       std::cerr << sick_timeout_exception.what() << std::endl;
01342       throw;
01343     }
01344     
01345     /* Handle any I/O exceptions */
01346     catch(SickIOException &sick_io_exception) {
01347       std::cerr << sick_io_exception.what() << std::endl;
01348       throw;
01349     }
01350 
01351     /* Handle any thread exceptions */
01352     catch(SickThreadException &sick_thread_exception) {
01353       std::cerr << sick_thread_exception.what() << std::endl;
01354       throw;
01355     }
01356     
01357     /* Handle anything else */
01358     catch(...) {
01359       std::cerr << "SickLMS2xx::GetSickScanSubrange: Unknown exception!!!" << std::endl;
01360       throw;
01361     }
01362 
01363   }
01364 
01388   void SickLMS2xx::GetSickPartialScan( unsigned int * const measurement_values,
01389                                     unsigned int & num_measurement_values,
01390                                     unsigned int & partial_scan_index,
01391                                     unsigned int * const sick_field_a_values,
01392                                     unsigned int * const sick_field_b_values,
01393                                     unsigned int * const sick_field_c_values,
01394                                     unsigned int * const sick_telegram_index,
01395                                     unsigned int * const sick_real_time_scan_index ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException) {
01396 
01397     /* Ensure the device is initialized */
01398     if (!_sick_initialized) {
01399       throw SickConfigException("SickLMS2xx::GetSickPartialScan: Sick LMS is not initialized!");
01400     }
01401     
01402     /* Declare message objects */
01403     SickLMS2xxMessage response;
01404 
01405     /* Declare some useful variables and a buffer */
01406     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01407     
01408     try {
01409 
01410       /* Restore original operating mode */
01411       _setSickOpModeMonitorStreamValuesFromPartialScan();
01412       
01413       /* Receive a data frame from the stream. */
01414       _recvMessage(response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT);
01415       
01416       /* Check that our payload has the proper command byte of 0xB0 */
01417       if(response.GetCommandCode() != 0xB0) {
01418         throw SickIOException("SickLMS2xx::GetSickPartialScan: Unexpected message!");
01419       }
01420 
01421       /* Acquire the payload buffer and length*/
01422       response.GetPayload(payload_buffer);
01423 
01424       /* Define a local scan profile object */
01425       sick_lms_2xx_scan_profile_b0_t sick_scan_profile;
01426 
01427       /* Initialize the profile */
01428       memset(&sick_scan_profile,0,sizeof(sick_lms_2xx_scan_profile_b0_t));
01429 
01430       /* Parse the message payload */
01431       _parseSickScanProfileB0(&payload_buffer[1],sick_scan_profile);
01432 
01433       /* Return the request values! */
01434       num_measurement_values = sick_scan_profile.sick_num_measurements;
01435 
01436       /* Assign the partial scan index */
01437       partial_scan_index = sick_scan_profile.sick_partial_scan_index;
01438       
01439       for (unsigned int i = 0; i < num_measurement_values; i++) {
01440 
01441         /* Copy the measurement value */
01442         measurement_values[i] = sick_scan_profile.sick_measurements[i];
01443 
01444         /* If requested, copy field A values */
01445         if(sick_field_a_values) {
01446           sick_field_a_values[i] = sick_scan_profile.sick_field_a_values[i];
01447         }
01448 
01449         /* If requested, copy field B values */
01450         if(sick_field_b_values) {
01451           sick_field_b_values[i] = sick_scan_profile.sick_field_b_values[i];
01452         }
01453 
01454         /* If requested, copy field C values */
01455         if(sick_field_c_values) {
01456           sick_field_c_values[i] = sick_scan_profile.sick_field_c_values[i];
01457         }
01458 
01459       }
01460 
01461       /* If requested, copy the real time scan index */
01462       if(sick_real_time_scan_index) {
01463         *sick_real_time_scan_index = sick_scan_profile.sick_real_time_scan_index;
01464       }
01465       
01466       /* If requested, copy the telegram index */
01467       if(sick_telegram_index) {
01468         *sick_telegram_index = sick_scan_profile.sick_telegram_index;
01469       }
01470 
01471     }
01472 
01473     /* Handle any config exceptions */
01474     catch(SickConfigException &sick_config_exception) {
01475       std::cerr << sick_config_exception.what() << std::endl;
01476       throw;
01477     }
01478     
01479     /* Handle a timeout exception */
01480     catch(SickTimeoutException &sick_timeout_exception) {
01481       std::cerr << sick_timeout_exception.what() << std::endl;
01482       throw;
01483     }
01484     
01485     /* Handle any I/O exceptions */
01486     catch(SickIOException &sick_io_exception) {
01487       std::cerr << sick_io_exception.what() << std::endl;
01488       throw;
01489     }
01490 
01491     /* Handle any thread exceptions */
01492     catch(SickThreadException &sick_thread_exception) {
01493       std::cerr << sick_thread_exception.what() << std::endl;
01494       throw;
01495     }
01496     
01497     /* Handle anything else */
01498     catch(...) {
01499       std::cerr << "SickLMS2xx::GetSickPartialScan: Unknown exception!!!" << std::endl;
01500       throw;
01501     }
01502 
01503   }
01504 
01519   void SickLMS2xx::GetSickMeanValues( const uint8_t sick_sample_size,
01520                                    unsigned int * const measurement_values,
01521                                    unsigned int & num_measurement_values,
01522                                    unsigned int * const sick_telegram_index,
01523                                    unsigned int * const sick_real_time_scan_index ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException) {
01524 
01525     /* Ensure the device is initialized */
01526     if (!_sick_initialized) {
01527       throw SickConfigException("SickLMS2xx::GetSickMeanValues: Sick LMS is not initialized!");
01528     }
01529     
01530     /* Declare message objects */
01531     SickLMS2xxMessage response;
01532 
01533     /* Declare some useful variables and a buffer */
01534     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01535     
01536     try {
01537 
01538       /* Restore original operating mode */
01539       _setSickOpModeMonitorStreamMeanValues(sick_sample_size);
01540       
01541       /* Receive a data frame from the stream. (NOTE: Can take 10+ seconds for a reply) */
01542       _recvMessage(response,DEFAULT_SICK_LMS_2XX_SICK_MEAN_VALUES_MESSAGE_TIMEOUT);
01543       
01544       /* Check that our payload has the proper command byte of 0xB0 */
01545       if(response.GetCommandCode() != 0xB6) {
01546         throw SickIOException("SickLMS2xx::GetSickMeanValues: Unexpected message!");
01547       }
01548 
01549       /* Acquire the payload buffer and length*/
01550       response.GetPayload(payload_buffer);
01551 
01552       /* Define a local scan profile object */
01553       sick_lms_2xx_scan_profile_b6_t sick_scan_profile;
01554 
01555       /* Initialize the profile */
01556       memset(&sick_scan_profile,0,sizeof(sick_lms_2xx_scan_profile_b6_t));
01557 
01558       /* Parse the message payload */
01559       _parseSickScanProfileB6(&payload_buffer[1],sick_scan_profile);
01560 
01561       /* Return the request values! */
01562       num_measurement_values = sick_scan_profile.sick_num_measurements;
01563 
01564       for (unsigned int i = 0; i < num_measurement_values; i++) {
01565         /* Copy the measurement value */
01566         measurement_values[i] = sick_scan_profile.sick_measurements[i];
01567       }
01568 
01569       /* If requested, copy the real time scan index */
01570       if(sick_real_time_scan_index) {
01571         *sick_real_time_scan_index = sick_scan_profile.sick_real_time_scan_index;
01572       }
01573       
01574       /* If requested, copy the telegram index */
01575       if(sick_telegram_index) {
01576         *sick_telegram_index = sick_scan_profile.sick_telegram_index;
01577       }
01578 
01579     }
01580 
01581     /* Handle any config exceptions */
01582     catch(SickConfigException &sick_config_exception) {
01583       std::cerr << sick_config_exception.what() << std::endl;
01584       throw;
01585     }
01586     
01587     /* Handle a timeout exception */
01588     catch(SickTimeoutException &sick_timeout_exception) {
01589       std::cerr << sick_timeout_exception.what() << std::endl;
01590       throw;
01591     }
01592     
01593     /* Handle any I/O exceptions */
01594     catch(SickIOException &sick_io_exception) {
01595       std::cerr << sick_io_exception.what() << std::endl;
01596       throw;
01597     }
01598 
01599     /* Handle any thread exceptions */
01600     catch(SickThreadException &sick_thread_exception) {
01601       std::cerr << sick_thread_exception.what() << std::endl;
01602       throw;
01603     }
01604     
01605     /* Handle anything else */
01606     catch(...) {
01607       std::cerr << "SickLMS2xx::GetSickMeanValues: Unknown exception!!!" << std::endl;
01608       throw;
01609     }
01610 
01611   }
01612 
01633   void SickLMS2xx::GetSickMeanValuesSubrange( const uint8_t sick_sample_size,
01634                                            const uint16_t sick_subrange_start_index,
01635                                            const uint16_t sick_subrange_stop_index,
01636                                            unsigned int * const measurement_values,
01637                                            unsigned int & num_measurement_values,
01638                                            unsigned int * const sick_telegram_index,
01639                                            unsigned int * const sick_real_time_scan_index ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException) {
01640 
01641     /* Ensure the device is initialized */
01642     if (!_sick_initialized) {
01643       throw SickConfigException("SickLMS2xx::GetSickMeanValuesSubrange: Sick LMS is not initialized!");
01644     }
01645     
01646     /* Declare message objects */
01647     SickLMS2xxMessage response;
01648 
01649     /* Declare some useful variables and a buffer */
01650     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01651     
01652     try {
01653     
01654       /* Restore original operating mode */
01655       _setSickOpModeMonitorStreamMeanValuesSubrange(sick_sample_size,sick_subrange_start_index,sick_subrange_stop_index);
01656       
01657       /* Receive a data frame from the stream. */
01658       _recvMessage(response,DEFAULT_SICK_LMS_2XX_SICK_MEAN_VALUES_MESSAGE_TIMEOUT);
01659 
01660       /* Check that our payload has the proper command byte of 0xB0 */
01661       if(response.GetCommandCode() != 0xBF) {
01662         throw SickIOException("SickLMS2xx::GetSickMeanValuesSubrange: Unexpected message!");
01663       }
01664 
01665       /* Acquire the payload buffer and length*/
01666       response.GetPayload(payload_buffer);
01667 
01668       /* Define a local scan profile object */
01669       sick_lms_2xx_scan_profile_bf_t sick_scan_profile;
01670 
01671       /* Initialize the profile */
01672       memset(&sick_scan_profile,0,sizeof(sick_lms_2xx_scan_profile_bf_t));
01673 
01674       /* Parse the message payload */
01675       _parseSickScanProfileBF(&payload_buffer[1],sick_scan_profile);
01676 
01677       /* Return the request values! */
01678       num_measurement_values = sick_scan_profile.sick_num_measurements;
01679 
01680       for (unsigned int i = 0; i < num_measurement_values; i++) {
01681 
01682         /* Copy the measurement value */
01683         measurement_values[i] = sick_scan_profile.sick_measurements[i];
01684 
01685       }
01686 
01687       /* If requested, copy the real time scan index */
01688       if(sick_real_time_scan_index) {
01689         *sick_real_time_scan_index = sick_scan_profile.sick_real_time_scan_index;
01690       }
01691       
01692       /* If requested, copy the telegram index */
01693       if(sick_telegram_index) {
01694         *sick_telegram_index = sick_scan_profile.sick_telegram_index;
01695       }
01696 
01697     }
01698 
01699     /* Handle any config exceptions */
01700     catch(SickConfigException &sick_config_exception) {
01701       std::cerr << sick_config_exception.what() << std::endl;
01702       throw;
01703     }
01704     
01705     /* Handle a timeout exception */
01706     catch(SickTimeoutException &sick_timeout_exception) {
01707       std::cerr << sick_timeout_exception.what() << std::endl;
01708       throw;
01709     }
01710     
01711     /* Handle any I/O exceptions */
01712     catch(SickIOException &sick_io_exception) {
01713       std::cerr << sick_io_exception.what() << std::endl;
01714       throw;
01715     }
01716 
01717     /* Handle any thread exceptions */
01718     catch(SickThreadException &sick_thread_exception) {
01719       std::cerr << sick_thread_exception.what() << std::endl;
01720       throw;
01721     }
01722     
01723     /* Handle anything else */
01724     catch(...) {
01725       std::cerr << "SickLMS2xx::GetMeanValuesSubrange: Unknown exception!!!" << std::endl;
01726       throw;
01727     }
01728 
01729   }
01730 
01738   sick_lms_2xx_status_t SickLMS2xx::GetSickStatus( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
01739 
01740     /* Ensure the device is initialized */
01741     if (!_sick_initialized) {
01742       throw SickConfigException("SickLMS2xx::GetSickStatus: Sick LMS is not initialized!");
01743     }
01744     
01745     try {
01746       
01747       /* Refresh the status info! */
01748       _getSickStatus();
01749 
01750     }
01751 
01752     /* Catch any timeout exceptions */
01753     catch(SickTimeoutException &sick_timeout_exception) {
01754       std::cerr << sick_timeout_exception.what() << std::endl;
01755       throw;
01756     }
01757       
01758     /* Catch any I/O exceptions */
01759     catch(SickIOException &sick_io_exception) {
01760       std::cerr << sick_io_exception.what() << std::endl;
01761       throw;
01762     }
01763     
01764     /* Catch any thread exceptions */
01765     catch(SickThreadException &sick_thread_exception) {
01766       std::cerr << sick_thread_exception.what() << std::endl;
01767       throw;
01768     }
01769     
01770     /* Catch anything else */
01771     catch(...) {
01772       std::cerr << "SickLMS2xx::GetSickStatus: Unknown exception!" << std::endl;
01773       throw;
01774     }
01775 
01776     /* Return the latest Sick status */
01777     return (sick_lms_2xx_status_t)_sick_operating_status.sick_device_status;
01778   }
01779 
01783   bool SickLMS2xx::IsSickLMS2xxFast() const throw(SickConfigException) {
01784 
01785     /* Ensure the device is initialized */
01786     if (!_sick_initialized) {
01787       throw SickConfigException("SickLMS2xx::IsSickLMS2xxFast: Sick LMS is not initialized!");
01788     }
01789     
01790     return (_sick_type == SICK_LMS_TYPE_211_S14 ||
01791             _sick_type == SICK_LMS_TYPE_221_S14 ||
01792             _sick_type == SICK_LMS_TYPE_291_S14);
01793 
01794   }
01795   
01800   void SickLMS2xx::ResetSick( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
01801 
01802     /* Ensure the device is initialized */
01803     if (!_sick_initialized) {
01804       throw SickConfigException("SickLMS2xx::ResetSick: Sick LMS is not initialized!");
01805     }
01806     
01807     SickLMS2xxMessage message,response;
01808     uint8_t payload[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01809 
01810     /* Construct the reset command */
01811     payload[0] = 0x10; // Request field reset
01812     message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload,1);
01813     
01814     std::cout << "\tResetting the device..." << std::endl;
01815     std::cout << "\tWaiting for Power on message..." << std::endl;
01816 
01817     try {
01818 
01819       /* Send the reset command and wait for the reply */
01820       _sendMessageAndGetReply(message,response,0x91,(unsigned int)60e6,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
01821 
01822       std::cout << "\t\tPower on message received!" << std::endl;
01823       std::cout << "\tWaiting for LMS Ready message..." << std::endl;
01824 
01825       /* Set terminal baud to the detected rate to get the LMS ready message */
01826       _setTerminalBaud(_baudToSickBaud(DEFAULT_SICK_LMS_2XX_SICK_BAUD));
01827 
01828       /* Receive the LMS ready message after power on */
01829       _recvMessage(response,(unsigned int)30e6);
01830       
01831       /* Verify the response */
01832       if(response.GetCommandCode() != 0x90) {
01833         std::cerr << "SickLMS2xx::ResetSick: Unexpected reply! (assuming device has been reset!)"  << std::endl;
01834       } else {
01835         std::cout << "\t\tLMS Ready message received!" << std::endl;
01836       }
01837       std::cout << std::endl;
01838 
01839       /* Reinitialize and sync the device */
01840       Initialize(_desired_session_baud);
01841 
01842     }
01843     
01844     /* Catch any timeout exceptions */
01845     catch(SickTimeoutException &sick_timeout_exception) {
01846       std::cerr << sick_timeout_exception.what() << std::endl;
01847       throw;
01848     }
01849     
01850     /* Catch any I/O exceptions */
01851     catch(SickIOException &sick_io_exception) {
01852       std::cerr << sick_io_exception.what() << std::endl;
01853       throw;
01854     }
01855     
01856     /* Catch any thread exceptions */
01857     catch(SickThreadException &sick_thread_exception) {
01858       std::cerr << sick_thread_exception.what() << std::endl;
01859       throw;
01860     }
01861     
01862     /* Catch anything else */
01863     catch(...) {
01864       std::cerr << "SickLMS2xx::ResetSick: Unknown exception!!!" << std::endl;
01865       throw;
01866     }
01867     
01868     std::cout << "\tRe-initialization sucessful. LMS is ready to go!" << std::endl;
01869     
01870   }
01871 
01876   std::string SickLMS2xx::GetSickStatusAsString( ) const {
01877 
01878     std::stringstream str_stream;
01879 
01880     str_stream << "\t=============== Sick LMS Status ===============" << std::endl;
01881 
01882     /* If Sick is initialized then print the status! */
01883     if (_sick_initialized) {
01884 
01885       str_stream << "\tVariant: " << _sickVariantToString(_sick_operating_status.sick_variant) << std::endl;
01886       str_stream << "\tSensor Status: " << SickStatusToString((sick_lms_2xx_status_t)_sick_operating_status.sick_device_status) << std::endl;
01887       str_stream << "\tScan Angle: " << GetSickScanAngle() << " (deg)" << std::endl;
01888       str_stream << "\tScan Resolution: " << GetSickScanResolution() << " (deg)" << std::endl;
01889       str_stream << "\tOperating Mode: " << SickOperatingModeToString(GetSickOperatingMode()) << std::endl;
01890       str_stream << "\tMeasuring Mode: " << SickMeasuringModeToString(GetSickMeasuringMode()) << std::endl;
01891       str_stream << "\tMeasuring Units: " << SickMeasuringUnitsToString(GetSickMeasuringUnits()) << std::endl;
01892 
01893     }
01894     else {
01895       
01896       str_stream << "\t Unknown (Device is not initialized)" << std::endl;
01897 
01898     }      
01899 
01900     str_stream << "\t===============================================" << std::endl;
01901     
01902     return str_stream.str();
01903   }
01904 
01909   std::string SickLMS2xx::GetSickSoftwareVersionAsString( ) const {
01910 
01911     std::stringstream str_stream;
01912     
01913     str_stream << "\t============== Sick LMS Software ==============" << std::endl;
01914 
01915     if (_sick_initialized) {
01916     
01917       str_stream << "\tSystem Software: " << std::string((char *)_sick_software_status.sick_system_software_version) << std::endl;
01918       str_stream << "\tSystem Boot PROM Software: " << std::string((char *)_sick_software_status.sick_prom_software_version) << std::endl;
01919 
01920     }
01921     else {
01922       
01923       str_stream << "\t Unknown (Device is not initialized)" << std::endl;
01924       
01925     }
01926     
01927     str_stream << "\t===============================================" << std::endl;
01928 
01929     return str_stream.str();
01930   }
01931 
01936   std::string SickLMS2xx::GetSickConfigAsString( ) const {
01937 
01938     std::stringstream str_stream;
01939 
01940     str_stream<< "\t=============== Sick LMS Config ===============" << std::endl;
01941 
01942     if (_sick_initialized) {
01943   
01944       str_stream << "\tBlanking Value: " << _sick_device_config.sick_blanking << std::endl;
01945       
01946       if(_isSickLMS211() || _isSickLMS221() || _isSickLMS291()) {
01947         str_stream << "\tSensitivity: " << SickSensitivityToString(GetSickSensitivity()) << std::endl;
01948       }
01949       else {
01950         str_stream << "\tPeak Thresh: " << SickPeakThresholdToString((sick_lms_2xx_peak_threshold_t)_sick_device_config.sick_peak_threshold) << std::endl;
01951         str_stream << "\tStop Thresh: " << (unsigned int)_sick_device_config.sick_stop_threshold << std::endl;
01952       }
01953     
01954       str_stream << "\tAvailability: " << _sickAvailabilityToString(_sick_device_config.sick_availability_level) << std::endl;
01955       str_stream << "\tMeasuring Mode: " << SickMeasuringModeToString((sick_lms_2xx_measuring_mode_t)_sick_device_config.sick_measuring_mode) << std::endl;
01956       str_stream << "\tMeasuring Units: " << SickMeasuringUnitsToString((sick_lms_2xx_measuring_units_t)_sick_device_config.sick_measuring_units) << std::endl;
01957       str_stream << "\tTemporary Field: " << _sickTemporaryFieldToString(_sick_device_config.sick_temporary_field) << std::endl;
01958       str_stream << "\tSubtractive Fields: " << _sickSubtractiveFieldsToString(_sick_device_config.sick_subtractive_fields) << std::endl;
01959       str_stream << "\tMultiple Evaluation: " << (unsigned int)_sick_device_config.sick_multiple_evaluation << std::endl;
01960       str_stream << "\tSuppressed Objects Multiple Evaluation: " << (unsigned int)_sick_device_config.sick_multiple_evaluation_suppressed_objects << std::endl;
01961       str_stream << "\tDazzling Multiple Evaluation: " << (unsigned int)_sick_device_config.sick_dazzling_multiple_evaluation << std::endl;
01962       str_stream << "\tRestart Mode: " << _sickRestartToString(_sick_device_config.sick_restart) << std::endl;
01963       str_stream << "\tRestart Time: " << (unsigned int)_sick_device_config.sick_restart_time << std::endl;
01964       str_stream << "\tFields B,C Restart Time: " << (unsigned int)_sick_device_config.sick_fields_b_c_restart_times << std::endl;
01965       str_stream << "\tContour Function A: " << _sickContourFunctionToString(_sick_device_config.sick_contour_a_reference) << std::endl;
01966       str_stream << "\tContour Function B: " << _sickContourFunctionToString(_sick_device_config.sick_contour_b_reference) << std::endl;
01967       str_stream << "\tContour Function C: " << _sickContourFunctionToString(_sick_device_config.sick_contour_c_reference) << std::endl;
01968       str_stream << "\tPixel Oriented Evaluation: " << (unsigned int)_sick_device_config.sick_pixel_oriented_evaluation << std::endl;
01969       str_stream << "\tSingle Measured Value Eval. Mode: " << (unsigned int)_sick_device_config.sick_single_measured_value_evaluation_mode << std::endl;
01970       
01971     }
01972     else {
01973       
01974       str_stream << "\t Unknown (Device is not initialized)" << std::endl;
01975       
01976     }
01977 
01978     str_stream << "\t===============================================" << std::endl;
01979     
01980     return str_stream.str();
01981   }
01982   
01986   void SickLMS2xx::PrintSickStatus() const {
01987     std::cout << GetSickStatusAsString() << std::endl;
01988   }
01989 
01993   void SickLMS2xx::PrintSickSoftwareVersion() const {
01994     std::cout << GetSickSoftwareVersionAsString() << std::endl;  
01995   }
01996 
02000   void SickLMS2xx::PrintSickConfig() const {
02001     std::cout << GetSickConfigAsString() << std::endl;
02002   }
02003   
02009   std::string SickLMS2xx::SickTypeToString( const sick_lms_2xx_type_t sick_type ) {
02010 
02011     switch(sick_type) {
02012     case SICK_LMS_TYPE_200_30106:
02013       return "Sick LMS 200-30106";
02014     case SICK_LMS_TYPE_211_30106:
02015       return "Sick LMS 211-30106";
02016     case SICK_LMS_TYPE_211_30206:
02017       return "Sick LMS 211-30206";
02018     case SICK_LMS_TYPE_211_S07:
02019       return "Sick LMS 211-S07";
02020     case SICK_LMS_TYPE_211_S14:
02021       return "Sick LMS 211-S14";
02022     case SICK_LMS_TYPE_211_S15:
02023       return "Sick LMS 211-S15";
02024     case SICK_LMS_TYPE_211_S19:
02025       return "Sick LMS 211-S19";
02026     case SICK_LMS_TYPE_211_S20:
02027       return "Sick LMS 211-S20";
02028     case SICK_LMS_TYPE_220_30106:
02029       return "Sick LMS 220-30106";
02030     case SICK_LMS_TYPE_221_30106:
02031       return "Sick LMS 221-30106";
02032     case SICK_LMS_TYPE_221_30206:
02033       return "Sick LMS 221-30206";
02034     case SICK_LMS_TYPE_221_S07:
02035       return "Sick LMS 221-S07";
02036     case SICK_LMS_TYPE_221_S14:
02037       return "Sick LMS 221-S14";
02038     case SICK_LMS_TYPE_221_S15:
02039       return "Sick LMS 221-S15";
02040     case SICK_LMS_TYPE_221_S16:
02041       return "Sick LMS 221-S16";
02042     case SICK_LMS_TYPE_221_S19:
02043       return "Sick LMS 221-S19";
02044     case SICK_LMS_TYPE_221_S20:
02045       return "Sick LMS 221-S20";
02046     case SICK_LMS_TYPE_291_S05:
02047       return "Sick LMS 291-S05";
02048     case SICK_LMS_TYPE_291_S14:
02049       return "Sick LMS 291-S14";
02050     case SICK_LMS_TYPE_291_S15:
02051       return "Sick LMS 291-S15";
02052     default:
02053       return "Unknown!";
02054     }
02055     
02056   }
02057 
02062   sick_lms_2xx_scan_angle_t SickLMS2xx::IntToSickScanAngle( const int scan_angle_int ) {
02063 
02064     switch(scan_angle_int) {
02065     case 90:
02066       return SICK_SCAN_ANGLE_90;
02067     case 100:
02068       return SICK_SCAN_ANGLE_100;
02069     case 180:
02070       return SICK_SCAN_ANGLE_180;
02071     default:
02072       return SICK_SCAN_ANGLE_UNKNOWN;      
02073     }
02074 
02075   }
02076   
02081   sick_lms_2xx_scan_resolution_t SickLMS2xx::IntToSickScanResolution( const int scan_resolution_int ) {
02082 
02083     switch(scan_resolution_int) {
02084     case 25:
02085       return SICK_SCAN_RESOLUTION_25;
02086     case 50:
02087       return SICK_SCAN_RESOLUTION_50;
02088     case 100:
02089       return SICK_SCAN_RESOLUTION_100;
02090     default:
02091       return SICK_SCAN_RESOLUTION_UNKNOWN;      
02092     }
02093 
02094   }
02095   
02100   sick_lms_2xx_scan_resolution_t SickLMS2xx::DoubleToSickScanResolution( const double scan_resolution_double ) {
02101     return IntToSickScanResolution((const int)(scan_resolution_double*100));
02102   }
02103   
02109   std::string SickLMS2xx::SickBaudToString( const sick_lms_2xx_baud_t baud_rate ) {
02110 
02111     switch(baud_rate) {
02112     case SICK_BAUD_9600:
02113       return "9600bps";
02114     case SICK_BAUD_19200:
02115       return "19200bps";
02116     case SICK_BAUD_38400:
02117       return "38400bps";
02118     case SICK_BAUD_500K:
02119       return "500Kbps";
02120     default:
02121       return "Unknown!";
02122     }
02123     
02124   }
02125 
02130   sick_lms_2xx_baud_t SickLMS2xx::IntToSickBaud( const int baud_int ) {
02131 
02132     switch(baud_int) {
02133     case 9600:
02134       return SICK_BAUD_9600;
02135     case 19200:
02136       return SICK_BAUD_19200;
02137     case 38400:
02138       return SICK_BAUD_38400;
02139     case 500000:
02140       return SICK_BAUD_500K;
02141     default:
02142       return SICK_BAUD_UNKNOWN;      
02143     }
02144 
02145   }
02146   
02151   sick_lms_2xx_baud_t SickLMS2xx::StringToSickBaud( const std::string baud_str ) {
02152 
02153     int baud_int;
02154     std::istringstream input_stream(baud_str);
02155     input_stream >> baud_int;
02156     
02157     return IntToSickBaud(baud_int);
02158 
02159   }
02160   
02166   std::string SickLMS2xx::SickStatusToString( const sick_lms_2xx_status_t sick_status ) {
02167 
02168     /* Return a string */
02169     if(sick_status != SICK_STATUS_OK) {
02170       return "Error (possibly fatal)";
02171     }  
02172     return "OK!";
02173     
02174   }
02175 
02181   std::string SickLMS2xx::SickMeasuringModeToString( const sick_lms_2xx_measuring_mode_t sick_measuring_mode ) {
02182 
02183     switch(sick_measuring_mode) {
02184     case SICK_MS_MODE_8_OR_80_FA_FB_DAZZLE:
02185       return "8m/80m; fields A,B,Dazzle";
02186     case SICK_MS_MODE_8_OR_80_REFLECTOR:
02187       return "8m/80m; 3 reflector bits";
02188     case SICK_MS_MODE_8_OR_80_FA_FB_FC:
02189       return "8m/80m; fields A,B,C";
02190     case SICK_MS_MODE_16_REFLECTOR:
02191       return "16m; 4 reflector bits";
02192     case SICK_MS_MODE_16_FA_FB:
02193       return "16m; fields A & B";
02194     case SICK_MS_MODE_32_REFLECTOR:
02195       return "32m; 2 reflector bits";
02196     case SICK_MS_MODE_32_FA:
02197       return "32m; field A";
02198     case SICK_MS_MODE_32_IMMEDIATE:
02199       return "32m; immediate";
02200     case SICK_MS_MODE_REFLECTIVITY:
02201       return "Reflectivity";
02202     default:
02203       return "Unknown";
02204     }  
02205   }
02206 
02212   std::string SickLMS2xx::SickOperatingModeToString( const sick_lms_2xx_operating_mode_t sick_operating_mode ) {
02213 
02214     switch(sick_operating_mode) {
02215     case SICK_OP_MODE_INSTALLATION:
02216       return "Installation Mode";
02217     case SICK_OP_MODE_DIAGNOSTIC:
02218       return "Diagnostic Mode";
02219     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUE_FOR_EACH_SEGMENT:
02220       return "Stream mim measured values for each segment";
02221     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VALUE_ON_OBJECT:
02222       return "Min measured value for each segment when object detected";
02223     case SICK_OP_MODE_MONITOR_STREAM_MIN_VERT_DIST_TO_OBJECT:
02224       return "Min vertical distance";
02225     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VERT_DIST_TO_OBJECT:
02226       return "Min vertical distance when object detected";
02227     case SICK_OP_MODE_MONITOR_STREAM_VALUES:
02228       return "Stream all measured values";
02229     case SICK_OP_MODE_MONITOR_REQUEST_VALUES:
02230       return "Request measured values";
02231     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES:
02232       return "Stream mean measured values";
02233     case SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE:
02234       return "Stream measured value subrange";
02235     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE:
02236       return "Stream mean measured value subrange";
02237     case SICK_OP_MODE_MONITOR_STREAM_VALUES_WITH_FIELDS:
02238       return "Stream measured and field values";
02239     case SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN:
02240       return "Stream measured values from partial scan";
02241     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT_FROM_PARTIAL_SCAN:
02242       return "Stream range w/ reflectivity from partial scan";
02243     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUES_FOR_EACH_SEGMENT_SUBRANGE:
02244       return "Stream min measured values for each segment over a subrange";
02245     case SICK_OP_MODE_MONITOR_NAVIGATION:
02246       return "Output navigation data records";
02247     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT:
02248       return "Stream range w/ reflectivity values";
02249     default:
02250       return "Unknown!";
02251     };
02252     
02253   }
02254   
02260   std::string SickLMS2xx::SickSensitivityToString( const sick_lms_2xx_sensitivity_t sick_sensitivity ) {
02261 
02262     switch(sick_sensitivity) {    
02263     case SICK_SENSITIVITY_STANDARD:
02264       return "Standard (~30m @ 10% reflectivity)";
02265     case SICK_SENSITIVITY_MEDIUM:
02266       return "Medium (~25m @ 10% reflectivity)";
02267     case SICK_SENSITIVITY_LOW:
02268       return "Low (~20m @ 10% relfectivity)";
02269     case SICK_SENSITIVITY_HIGH:
02270       return "High (~42m @ 10% reflectivity)";
02271     default:
02272       return "Unknown!";
02273     }
02274 
02275   }
02276 
02282   std::string SickLMS2xx::SickPeakThresholdToString( const sick_lms_2xx_peak_threshold_t sick_peak_threshold ) {
02283 
02284     switch(sick_peak_threshold) {    
02285     case SICK_PEAK_THRESHOLD_DETECTION_WITH_NO_BLACK_EXTENSION:
02286       return "Peak detection, no black extension";
02287     case SICK_PEAK_THRESHOLD_DETECTION_WITH_BLACK_EXTENSION:
02288       return "Peak detection w/ black extension";
02289     case SICK_PEAK_THRESHOLD_NO_DETECTION_WITH_NO_BLACK_EXTENSION:
02290       return "No peak detection, no black extension";
02291     case SICK_PEAK_THRESHOLD_NO_DETECTION_WITH_BLACK_EXTENSION:
02292       return "No peak detection w/ black extension";
02293     default:
02294       return "Unknown!";
02295     }
02296 
02297   }
02298   
02304   std::string SickLMS2xx::SickMeasuringUnitsToString( const sick_lms_2xx_measuring_units_t sick_units ) {
02305 
02306     /* Return the proper string */
02307     switch(sick_units) {
02308     case SICK_MEASURING_UNITS_CM:
02309       return "Centimeters (cm)";
02310     case SICK_MEASURING_UNITS_MM:
02311       return "Millimeters (mm)";
02312     default:
02313       return "Unknown!";
02314     }
02315     
02316   }
02317 
02321    void SickLMS2xx::_setupConnection() throw ( SickIOException, SickThreadException ) {
02322      SickLMS2xx::_setupConnection(0);
02323    }
02324   
02329   void SickLMS2xx::_setupConnection( const uint32_t delay ) throw ( SickIOException, SickThreadException ) {
02330 
02331     try {
02332     
02333       /* Open the device */
02334       if((_sick_fd = open(_sick_device_path.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)) < 0) {
02335         throw SickIOException("SickLMS2xx::_setupConnection: - Unable to open serial port");
02336       }
02337 
02338       // Sleep to allow the SICK to power on for some applications
02339       sleep(delay);
02340       
02341       /* Backup the original term settings */
02342       if(tcgetattr(_sick_fd,&_old_term) < 0) {
02343         throw SickIOException("SickLMS2xx::_setupConnection: tcgetattr() failed!");
02344       }
02345 
02346       /* Set the host terminal baud rate to the new speed */
02347       _setTerminalBaud(_baudToSickBaud(DEFAULT_SICK_LMS_2XX_SICK_BAUD));
02348       
02349     }
02350 
02351     /* Handle any I/O exceptions */
02352     catch(SickIOException &sick_io_exception) {
02353       std::cerr << sick_io_exception.what() << std::endl;
02354       throw;
02355     }
02356 
02357     /* Handle any thread exceptions */
02358     catch(SickThreadException &sick_thread_exception) {
02359       std::cerr << sick_thread_exception.what() << std::endl;
02360       throw;
02361     }
02362 
02363     /* Handle unknown exceptions */
02364     catch(...) {
02365       std::cerr << "SickLMS2xx::_setupConnection: Unknown exception!" << std::endl;
02366       throw;
02367     }
02368     
02369   }
02370 
02374   void SickLMS2xx::_teardownConnection( ) throw( SickIOException ) {
02375 
02376     /* Check whether device was initialized */
02377     if(!_sick_initialized) {
02378       return;
02379     }
02380     
02381     /* Restore old terminal settings */
02382     if (tcsetattr(_sick_fd,TCSANOW,&_old_term) < 0) {
02383       throw SickIOException("SickLMS2xx::_teardownConnection: tcsetattr() failed!");
02384     }
02385 
02386     /* Actually close the device */
02387     if(close(_sick_fd) != 0) {
02388       throw SickIOException("SickLMS2xx::_teardownConnection: close() failed!");
02389     }
02390 
02391   }
02392 
02396   void SickLMS2xx::_flushTerminalBuffer( ) throw ( SickThreadException ) {
02397 
02398     try {
02399     
02400       /* Acquire access to the data stream */    
02401       _sick_buffer_monitor->AcquireDataStream();
02402 
02403       /* Nobody is reading a message, so safely flush! */
02404       if (tcflush(_sick_fd,TCIOFLUSH) != 0) {
02405         throw SickThreadException("SickLMS2xx::_flushTerminalBuffer: tcflush() failed!");
02406       }
02407       
02408       /* Attempt to release the data stream */
02409       _sick_buffer_monitor->ReleaseDataStream();
02410 
02411     }
02412 
02413     /* Handle thread exceptions */
02414     catch(SickThreadException &sick_thread_exception) {
02415       std::cerr << sick_thread_exception.what() << std::endl;
02416       throw;
02417     }
02418 
02419     /* A sanity check */
02420     catch(...) {
02421       std::cerr << "SickLMS2xx::_flushTerminalBuffer: Unknown exception!" << std::endl;
02422       throw;
02423     }
02424 
02425   }
02426 
02437   void SickLMS2xx::_sendMessageAndGetReply( const SickLMS2xxMessage &send_message,
02438                                             SickLMS2xxMessage &recv_message,
02439                                             const unsigned int timeout_value,
02440                                             const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException ) {
02441 
02442     uint8_t sick_reply_code = send_message.GetCommandCode() + 0x80;
02443     
02444     try {
02445 
02446       /* Send a message and get reply using a reply code */
02447       _sendMessageAndGetReply(send_message,recv_message,sick_reply_code,timeout_value,num_tries);
02448 
02449     }
02450     
02451     /* Handle a timeout! */
02452     catch (SickTimeoutException &sick_timeout) {
02453       /* For now just rethrow it */
02454       throw;
02455     }
02456     
02457     /* Handle a thread exception */
02458     catch (SickThreadException &sick_thread_exception) {
02459       std::cerr << sick_thread_exception.what() << std::endl;
02460       throw;
02461     }
02462     
02463     /* Handle write buffer exceptions */
02464     catch (SickIOException &sick_io_error) {
02465       std::cerr << sick_io_error.what() << std::endl;
02466       throw;
02467     }
02468     
02469     /* A safety net */
02470     catch (...) {
02471       std::cerr << "SickLMS2xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
02472       throw;
02473     }
02474     
02475   }
02476   
02485   void SickLMS2xx::_sendMessageAndGetReply( const SickLMS2xxMessage &send_message,
02486                                             SickLMS2xxMessage &recv_message,
02487                                             const uint8_t reply_code,
02488                                             const unsigned int timeout_value,
02489                                             const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException ) {
02490 
02491     try {
02492 
02493       /* Attempt to flush the terminal buffer */
02494       _flushTerminalBuffer();
02495       
02496       /* Send a message and get reply using parent's method */
02497       SickLIDAR< SickLMS2xxBufferMonitor, SickLMS2xxMessage >::_sendMessageAndGetReply(send_message,recv_message,&reply_code,1,DEFAULT_SICK_LMS_2XX_BYTE_INTERVAL,timeout_value,num_tries);
02498 
02499     }
02500     
02501     /* Handle a timeout! */
02502     catch (SickTimeoutException &sick_timeout) {
02503       throw;
02504     }
02505     
02506     /* Handle a thread exception */
02507     catch (SickThreadException &sick_thread_exception) {
02508       std::cerr << sick_thread_exception.what() << std::endl;
02509       throw;
02510     }
02511     
02512     /* Handle write buffer exceptions */
02513     catch (SickIOException &sick_io_error) {
02514       std::cerr << sick_io_error.what() << std::endl;
02515       throw;
02516     }
02517     
02518     /* A safety net */
02519     catch (...) {
02520       std::cerr << "SickLMS2xx::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
02521       throw;
02522     }
02523     
02524   }
02525   
02530   void SickLMS2xx::_setSessionBaud(const sick_lms_2xx_baud_t baud_rate) throw ( SickIOException, SickThreadException, SickTimeoutException ){
02531     
02532     SickLMS2xxMessage message, response;
02533     
02534     uint8_t payload[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02535     
02536     /* Another sanity check */
02537     if(baud_rate == SICK_BAUD_UNKNOWN) {
02538       throw SickIOException("SickLMS2xx::_setSessionBaud: Undefined baud rate!");
02539     }    
02540     
02541     /* Construct the command telegram */
02542     payload[0] = 0x20;
02543     payload[1] = baud_rate;
02544     
02545     message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload,2);
02546     
02547     try {
02548 
02549       /* Send the status request and get a reply */
02550       _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
02551 
02552       /* Set the host terminal baud rate to the new speed */
02553       _setTerminalBaud(baud_rate);
02554 
02555       /* Sick likes a sleep here */
02556       usleep(250000);
02557       
02558     }
02559     
02560     /* Catch a timeout */
02561     catch(SickTimeoutException &sick_timeout_exception) {
02562       std::cerr << sick_timeout_exception.what() << std::endl;
02563       throw;
02564     }
02565     
02566     /* Catch any I/O exceptions */
02567     catch(SickIOException &sick_io_exception) {
02568       std::cerr << sick_io_exception.what() << std::endl;
02569       throw;
02570     }
02571     
02572     /* Catch any thread exceptions */
02573     catch(SickThreadException &sick_thread_exception) {
02574       std::cerr << sick_thread_exception.what() << std::endl;
02575       throw;
02576     }
02577     
02578     /* Catch anything else */
02579     catch(...) {
02580       std::cerr << "SickLMS2xx::_getSickErrors: Unknown exception!!!" << std::endl;
02581       throw;
02582     }
02583 
02584   }
02585   
02590   bool SickLMS2xx::_testSickBaud(const sick_lms_2xx_baud_t baud_rate) throw( SickIOException, SickThreadException ) {
02591 
02592     try {
02593     
02594       /* Another sanity check */
02595       if(baud_rate == SICK_BAUD_UNKNOWN) {
02596         throw SickIOException("SickLMS2xx::_testBaudRate: Undefined baud rate!");
02597       }
02598       
02599       /* Attempt to get status information at the current baud */
02600       std::cout << "\t\tChecking " << SickBaudToString(baud_rate) << "..." << std::endl;
02601       
02602       /* Set the host terminal baud rate to the test speed */
02603       _setTerminalBaud(baud_rate);
02604       
02605       try {
02606 
02607         /* Check to see if the Sick replies! */
02608         _getSickErrors();
02609 
02610       }
02611 
02612       /* Catch a timeout exception */
02613       catch(SickTimeoutException &sick_timeout_exception) {
02614         /* This means that the current baud rate timed out! */
02615         return false;
02616       }
02617 
02618       /* Catch anything else and throw it away */
02619       catch(...) {
02620         std::cerr << "SickLMS2xx::_testBaudRate: Unknown exception!" << std::endl;
02621         throw;
02622       }
02623       
02624     }
02625 
02626     /* Handle any IO exceptions */
02627     catch(SickIOException &sick_io_exception) {
02628       std::cerr << sick_io_exception.what() << std::endl;
02629       throw;
02630     }
02631 
02632     /* Handle thread exceptions */
02633     catch(SickThreadException &sick_thread_exception) {
02634       std::cerr << sick_thread_exception.what() << std::endl;
02635       throw; 
02636     }
02637 
02638     /* A safety net */
02639     catch(...) {
02640       std::cerr << "SickLMS2xx::_testBaudRate: Unknown exception!!!" << std::endl;
02641       throw; 
02642     }
02643 
02644     /* Success! */
02645     return true;
02646 
02647   }
02648 
02653   void SickLMS2xx::_setTerminalBaud( const sick_lms_2xx_baud_t baud_rate ) throw( SickIOException, SickThreadException ) {
02654 
02655     struct termios term;
02656 
02657 #ifdef HAVE_LINUX_SERIAL_H
02658     struct serial_struct serial;
02659 #endif
02660     
02661     try {
02662     
02663       /* If seeting baud to 500k */
02664       if (baud_rate == SICK_BAUD_500K) {
02665 
02666 #ifdef HAVE_LINUX_SERIAL_H
02667         
02668         /* Get serial attributes */
02669         if(ioctl(_sick_fd,TIOCGSERIAL,&serial) < 0) {
02670           throw SickIOException("SickLMS2xx::_setTerminalBaud: ioctl() failed!");
02671         }
02672         
02673         /* Set the custom devisor */
02674         serial.flags |= ASYNC_SPD_CUST;
02675         serial.custom_divisor = 48; // for FTDI USB/serial converter divisor is 240/5
02676         
02677         /* Set the new attibute values */
02678         if(ioctl(_sick_fd,TIOCSSERIAL,&serial) < 0) {
02679           throw SickIOException("SickLMS2xx::_setTerminalBaud: ioctl() failed!");
02680         }
02681 
02682 #else
02683         throw SickIOException("SickLMS2xx::_setTerminalBaud - 500K baud is only supported under Linux!");
02684 #endif
02685         
02686       }
02687 
02688 #ifdef HAVE_LINUX_SERIAL_H
02689       
02690       else { /* Using a standard baud rate */
02691 
02692         /* We let the next few errors slide in case USB adapter is being used */
02693         if(ioctl(_sick_fd,TIOCGSERIAL,&serial) < 0) {
02694           std::cerr << "SickLMS2xx::_setTermSpeed: ioctl() failed while trying to get serial port info!" << std::endl;
02695           std::cerr << "\tNOTE: This is normal when connected via USB!" <<std::endl;
02696         }
02697         
02698         serial.custom_divisor = 0;
02699         serial.flags &= ~ASYNC_SPD_CUST;
02700         
02701         if(ioctl(_sick_fd,TIOCSSERIAL,&serial) < 0) {
02702           std::cerr << "SickLMS2xx::_setTerminalBaud: ioctl() failed while trying to set serial port info!" << std::endl;
02703           std::cerr << "\tNOTE: This is normal when connected via USB!" <<std::endl;
02704         }
02705         
02706       }
02707       
02708 #endif
02709       
02710       /* Attempt to acquire device attributes */
02711       if(tcgetattr(_sick_fd,&term) < 0) {
02712         throw SickIOException("SickLMS2xx::_setTerminalBaud: Unable to get device attributes!");
02713       }
02714       
02715       /* Switch on the baud rate */
02716       switch(baud_rate) {      
02717       case SICK_BAUD_9600: {
02718         cfmakeraw(&term);
02719         cfsetispeed(&term,B9600);
02720         cfsetospeed(&term,B9600);
02721         break;
02722       }
02723       case SICK_BAUD_19200: {
02724         cfmakeraw(&term);
02725         cfsetispeed(&term,B19200);
02726         cfsetospeed(&term,B19200);
02727         break;
02728       }
02729       case SICK_BAUD_38400: {
02730         cfmakeraw(&term);
02731         cfsetispeed(&term,B38400);
02732         cfsetospeed(&term,B38400);            
02733         break;
02734       }
02735       case SICK_BAUD_500K: {      
02736         cfmakeraw(&term);
02737         cfsetispeed(&term,B38400);
02738         cfsetospeed(&term,B38400);
02739         break;
02740       }
02741       default:
02742         throw SickIOException("SickLMS2xx::_setTerminalBaud: Unknown baud rate!");
02743       }
02744       
02745       /* Attempt to set the device attributes */
02746       if(tcsetattr(_sick_fd,TCSAFLUSH,&term) < 0 ) {
02747         throw SickIOException("SickLMS2xx::_setTerminalBaud: Unable to set device attributes!");
02748       }
02749       
02750       /* Buffer the rate locally */
02751       _curr_session_baud = baud_rate;
02752       
02753       /* Attempt to flush the I/O buffers */
02754       _flushTerminalBuffer();
02755       
02756     } // try
02757 
02758     /* Catch an IO exception */
02759     catch(SickIOException sick_io_exception) {
02760       std::cerr << sick_io_exception.what() << std::endl;
02761       throw;
02762     }
02763     
02764     /* Catch an IO exception */
02765     catch(SickThreadException sick_thread_exception) {
02766       std::cerr << sick_thread_exception.what() << std::endl;
02767       throw;
02768     }
02769     
02770     /* A sanity check */
02771     catch(...) {
02772       std::cerr << "SickLMS2xx::_setTerminalBaud: Unknown exception!!!" << std::endl;
02773       throw;
02774     }
02775 
02776   }
02777 
02781   void SickLMS2xx::_getSickType( ) throw( SickTimeoutException, SickIOException, SickThreadException ) {
02782     
02783     SickLMS2xxMessage message,response;
02784     
02785     int payload_length;
02786     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02787     
02788     /* Get the LMS type */
02789     payload_buffer[0] = 0x3A; //Command to request LMS type
02790     
02791     /* Build the message */
02792     message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,1);
02793 
02794     try {
02795        
02796       /* Send the status request and get a reply */
02797       _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
02798       
02799     }
02800     
02801     /* Catch any timeout exceptions */
02802     catch(SickTimeoutException &sick_timeout_exception) {
02803       std::cerr << sick_timeout_exception.what() << std::endl;
02804       throw;
02805     }
02806     
02807     /* Catch any I/O exceptions */
02808     catch(SickIOException &sick_io_exception) {
02809       std::cerr << sick_io_exception.what() << std::endl;
02810       throw;
02811     }
02812     
02813     /* Catch any thread exceptions */
02814     catch(SickThreadException &sick_thread_exception) {
02815       std::cerr << sick_thread_exception.what() << std::endl;
02816       throw;
02817     }
02818     
02819     /* Catch anything else */
02820     catch(...) {
02821       std::cerr << "SickLMS2xx::_getSickType: Unknown exception!!!" << std::endl;
02822       throw;
02823     }
02824     
02825     /* Reset the buffer */
02826     memset(payload_buffer,0,1);
02827   
02828     /* Get the payload */
02829     response.GetPayload(payload_buffer);
02830     
02831     /* Acquire the payload length */
02832     payload_length = response.GetPayloadLength();
02833     
02834     /* Dynamically allocate the string length */
02835     char * string_buffer = new char[payload_length-1];
02836 
02837     /* Initialize the buffer */
02838     memset(string_buffer,0,payload_length-1);
02839     memcpy(string_buffer,&payload_buffer[1],payload_length-2);
02840 
02841     /* Convert to a standard string */
02842     std::string type_string = string_buffer;
02843 
02844     /* Set the Sick LMS type in the driver */
02845     if(type_string.find("LMS200;30106") != std::string::npos) {
02846       _sick_type = SICK_LMS_TYPE_200_30106;
02847     } else if(type_string.find("LMS211;30106") != std::string::npos) {
02848       _sick_type = SICK_LMS_TYPE_211_30106;
02849     } else if(type_string.find("LMS211;30206") != std::string::npos) {
02850       _sick_type = SICK_LMS_TYPE_211_30206;
02851     } else if(type_string.find("LMS211;S07") != std::string::npos) {
02852       _sick_type = SICK_LMS_TYPE_211_S07;
02853     } else if(type_string.find("LMS211;S14") != std::string::npos) {
02854       _sick_type = SICK_LMS_TYPE_211_S14;
02855     } else if(type_string.find("LMS211;S15") != std::string::npos) {
02856       _sick_type = SICK_LMS_TYPE_211_S15;
02857     } else if(type_string.find("LMS211;S19") != std::string::npos) {
02858       _sick_type = SICK_LMS_TYPE_211_S19;
02859     } else if(type_string.find("LMS211;S20") != std::string::npos) {
02860       _sick_type = SICK_LMS_TYPE_211_S20;
02861     } else if(type_string.find("LMS220;30106") != std::string::npos) {
02862       _sick_type = SICK_LMS_TYPE_220_30106;
02863     } else if(type_string.find("LMS221;30106") != std::string::npos) {
02864       _sick_type = SICK_LMS_TYPE_221_30106;
02865     } else if(type_string.find("LMS221;30206") != std::string::npos) {
02866       _sick_type = SICK_LMS_TYPE_221_30206;
02867     } else if(type_string.find("LMS221;S07") != std::string::npos) {
02868       _sick_type = SICK_LMS_TYPE_221_S07;
02869     } else if(type_string.find("LMS221;S14") != std::string::npos) {
02870       _sick_type = SICK_LMS_TYPE_221_S14;
02871     } else if(type_string.find("LMS221;S15") != std::string::npos) {
02872       _sick_type = SICK_LMS_TYPE_221_S15;
02873     } else if(type_string.find("LMS221;S16") != std::string::npos) {
02874       _sick_type = SICK_LMS_TYPE_221_S16;
02875     } else if(type_string.find("LMS221;S19") != std::string::npos) {
02876       _sick_type = SICK_LMS_TYPE_221_S19;
02877     } else if(type_string.find("LMS221;S20") != std::string::npos) {
02878       _sick_type = SICK_LMS_TYPE_221_S20;
02879     } else if(type_string.find("LMS291;S05") != std::string::npos) {
02880       _sick_type = SICK_LMS_TYPE_291_S05;
02881     } else if(type_string.find("LMS291;S14") != std::string::npos) {
02882       _sick_type = SICK_LMS_TYPE_291_S14;
02883     } else if(type_string.find("LMS291;S15") != std::string::npos) {
02884       _sick_type = SICK_LMS_TYPE_291_S15;
02885     } else {
02886       _sick_type = SICK_LMS_TYPE_UNKNOWN;
02887     }
02888 
02889     /* Reclaim the allocated string buffer */
02890     if (string_buffer) {
02891       delete [] string_buffer;
02892     }
02893     
02894   }
02895 
02899   void SickLMS2xx::_getSickConfig( ) throw( SickTimeoutException, SickIOException, SickThreadException ) {
02900 
02901      SickLMS2xxMessage message, response;
02902 
02903      uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};    
02904 
02905      /* Set the command code */
02906      payload_buffer[0] = 0x74;
02907 
02908      /* Build the request message */
02909      message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,1);
02910 
02911      try {
02912        
02913        /* Send the status request and get a reply */
02914        _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
02915        
02916      }
02917 
02918      /* Catch any timeout exceptions */
02919      catch(SickTimeoutException &sick_timeout_exception) {
02920        std::cerr << sick_timeout_exception.what() << std::endl;
02921        throw;
02922      }
02923      
02924      /* Catch any I/O exceptions */
02925      catch(SickIOException &sick_io_exception) {
02926        std::cerr << sick_io_exception.what() << std::endl;
02927        throw;
02928      }
02929      
02930      /* Catch any thread exceptions */
02931      catch(SickThreadException &sick_thread_exception) {
02932        std::cerr << sick_thread_exception.what() << std::endl;
02933        throw;
02934      }
02935      
02936      /* Catch anything else */
02937      catch(...) {
02938        std::cerr << "SickLMS2xx::_getSickConfig: Unknown exception!!!" << std::endl;
02939        throw;
02940      }
02941 
02942      /* Reset the payload buffer */
02943      payload_buffer[0] = 0;
02944 
02945      /* Extract the payload */
02946      response.GetPayload(payload_buffer);
02947 
02948      /* Obtain the configuration results */
02949      _parseSickConfigProfile(&payload_buffer[1],_sick_device_config);
02950      
02951   }
02952 
02957   void SickLMS2xx::_setSickConfig( const sick_lms_2xx_device_config_t &sick_device_config )
02958     throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException ) {
02959 
02960     try {
02961       
02962       std::cout << "\tAttempting to configure the device (this can take a few seconds)..." << std::endl;
02963       
02964       /* Attempt to set the Sick into installation mode */
02965       _setSickOpModeInstallation();
02966       
02967       /* Define our message objects */
02968       SickLMS2xxMessage message, response;    
02969       uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};    
02970       
02971       /* Set the command code */
02972       payload_buffer[0] = 0x77; // Command to configure device
02973       
02974       /*
02975        * Build the corresponding config message
02976        */
02977       
02978       /* Set Block A */
02979       uint16_t temp_buffer = host_to_sick_lms_2xx_byte_order(sick_device_config.sick_blanking);
02980       memcpy(&payload_buffer[1],&temp_buffer,2);
02981       
02982       /* Set Block B */
02983       payload_buffer[3] = sick_device_config.sick_stop_threshold; // NOTE: This value will be 0 for LMS 211/221/291
02984       payload_buffer[4] = sick_device_config.sick_peak_threshold; // NOTE: This value is equivalent to sensitivity field on 211/221/291
02985   
02986       /* Set Block C */
02987       payload_buffer[5] = sick_device_config.sick_availability_level;
02988       
02989       /* Set Block D */
02990       payload_buffer[6] = sick_device_config.sick_measuring_mode;
02991       
02992       /* Set Block E */
02993       payload_buffer[7] = sick_device_config.sick_measuring_units;
02994       
02995       /* Set Block F */
02996       payload_buffer[8] = sick_device_config.sick_temporary_field;
02997       
02998       /* Set Block G */
02999       payload_buffer[9] = sick_device_config.sick_subtractive_fields;
03000       
03001       /* Set Block H */
03002       payload_buffer[10] = sick_device_config.sick_multiple_evaluation;
03003       
03004       /* Set Block I */
03005       payload_buffer[11] = sick_device_config.sick_restart;
03006       
03007       /* Set Block J */
03008       payload_buffer[12] = sick_device_config.sick_restart_time;
03009       
03010       /* Set Block K */
03011       payload_buffer[13] = sick_device_config.sick_multiple_evaluation_suppressed_objects;
03012       
03013       /* Set Block L */
03014       payload_buffer[14] = sick_device_config.sick_contour_a_reference;
03015     
03016       /* Set Block M */
03017       payload_buffer[15] = sick_device_config.sick_contour_a_positive_tolerance_band;
03018       
03019       /* Set Block N */
03020       payload_buffer[16] = sick_device_config.sick_contour_a_negative_tolerance_band;
03021       
03022       /* Set Block O */
03023       payload_buffer[17] = sick_device_config.sick_contour_a_start_angle;
03024       
03025       /* Set Block P */
03026       payload_buffer[18] = sick_device_config.sick_contour_a_stop_angle;
03027       
03028       /* Set Block Q */
03029       payload_buffer[19] = sick_device_config.sick_contour_b_reference;
03030       
03031       /* Set Block R */
03032       payload_buffer[20] = sick_device_config.sick_contour_b_positive_tolerance_band;
03033       
03034       /* Set Block S */
03035       payload_buffer[21] = sick_device_config.sick_contour_b_negative_tolerance_band;
03036       
03037       /* Set Block T */
03038       payload_buffer[22] = sick_device_config.sick_contour_b_start_angle;
03039     
03040       /* Set Block U */
03041       payload_buffer[23] = sick_device_config.sick_contour_b_stop_angle;
03042       
03043       /* Set Block V */
03044       payload_buffer[24] = sick_device_config.sick_contour_c_reference;
03045       
03046       /* Set Block W */
03047       payload_buffer[25] = sick_device_config.sick_contour_c_positive_tolerance_band;
03048       
03049       /* Set Block X */
03050       payload_buffer[26] = sick_device_config.sick_contour_c_negative_tolerance_band;
03051       
03052       /* Set Block Y */
03053       payload_buffer[27] = sick_device_config.sick_contour_c_start_angle;
03054       
03055       /* Set Block Z */
03056       payload_buffer[28] = sick_device_config.sick_contour_c_stop_angle;
03057       
03058       /* Set Block A1 */
03059       payload_buffer[29] = sick_device_config.sick_pixel_oriented_evaluation;
03060       
03061       /* Set Block A2 */
03062       payload_buffer[30] = sick_device_config.sick_single_measured_value_evaluation_mode;
03063       
03064       /* Set Block A3 */
03065       temp_buffer = host_to_sick_lms_2xx_byte_order(sick_device_config.sick_fields_b_c_restart_times);
03066       memcpy(&payload_buffer[31],&temp_buffer,2);
03067       
03068       /* Set Block A4 */
03069       temp_buffer = host_to_sick_lms_2xx_byte_order(sick_device_config.sick_dazzling_multiple_evaluation);
03070       memcpy(&payload_buffer[33],&temp_buffer,2);
03071       
03072       /* Populate the message container */
03073       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,35);
03074       
03075       /* Send the status request and get a reply */
03076       _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_CONFIG_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
03077 
03078       /* Reset the payload buffer */
03079       memset(payload_buffer,0,35);
03080 
03081       /* Extract the payload contents */
03082       response.GetPayload(payload_buffer);
03083 
03084       /* Check whether the configuration was successful */
03085       if (payload_buffer[1] != 0x01) {
03086         throw SickConfigException("SickLMS2xx::_setSickConfig: Configuration failed!");
03087       }
03088 
03089       /* Success */
03090       std::cout << "\t\tConfiguration successful! :o)" << std::endl;
03091 
03092       /* Update the local configuration data */
03093       _parseSickConfigProfile(&payload_buffer[2],_sick_device_config);    
03094       
03095       /* Set the device back to request range mode */
03096       _setSickOpModeMonitorRequestValues();
03097 
03098       /* Refresh the status info! */
03099       _getSickStatus();
03100       
03101     }
03102     
03103     /* Catch any timeout exceptions */
03104     catch(SickTimeoutException &sick_timeout_exception) {
03105       std::cerr << sick_timeout_exception.what() << std::endl;
03106       throw;
03107     }
03108 
03109     /* Catch any config exceptions */
03110     catch(SickConfigException &sick_config_exception) {
03111       std::cerr << sick_config_exception.what() << std::endl;
03112       throw;
03113     }
03114       
03115     /* Catch any I/O exceptions */
03116     catch(SickIOException &sick_io_exception) {
03117       std::cerr << sick_io_exception.what() << std::endl;
03118       throw;
03119     }
03120     
03121     /* Catch any thread exceptions */
03122     catch(SickThreadException &sick_thread_exception) {
03123       std::cerr << sick_thread_exception.what() << std::endl;
03124       throw;
03125     }
03126     
03127     /* Catch anything else */
03128     catch(...) {
03129       std::cerr << "SickLMS2xx::_setSickConfig: Unknown exception!" << std::endl;
03130       throw;
03131     }
03132 
03133   }
03134   
03138   void SickLMS2xx::_getSickErrors( unsigned int * const num_sick_errors, uint8_t * const error_type_buffer,
03139                                 uint8_t * const error_num_buffer ) throw( SickTimeoutException, SickIOException, SickThreadException ) {
03140 
03141      SickLMS2xxMessage message, response;
03142 
03143      int payload_length;
03144      uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
03145   
03146      /* The command to request LMS status */
03147      payload_buffer[0] = 0x32;
03148      
03149      /* Build the request message */
03150      message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,1);
03151      
03152      try {
03153        
03154        /* Send the status request and get a reply */
03155        _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
03156        
03157      }
03158 
03159      /* Catch any timeout exceptions */
03160      catch(SickTimeoutException &sick_timeout_exception) {
03161        std::cerr << sick_timeout_exception.what() << std::endl;
03162        throw;
03163      }
03164      
03165      /* Catch any I/O exceptions */
03166      catch(SickIOException &sick_io_exception) {
03167        std::cerr << sick_io_exception.what() << std::endl;
03168        throw;
03169      }
03170      
03171      /* Catch any thread exceptions */
03172      catch(SickThreadException &sick_thread_exception) {
03173        std::cerr << sick_thread_exception.what() << std::endl;
03174        throw;
03175      }
03176      
03177      /* Catch anything else */
03178      catch(...) {
03179        std::cerr << "SickLMS2xx::_getSickErrors: Unknown exception!!!" << std::endl;
03180        throw;
03181      }
03182      
03183      /* Extract the payload_length */
03184      payload_length = response.GetPayloadLength();
03185      
03186      /* Compute the number of errors */
03187      double num_errors = ((payload_length-2)/((double)2));
03188      
03189      /* Assign the number of errors if necessary */
03190      if (num_sick_errors) {
03191        *num_sick_errors = (unsigned int)num_errors;
03192      }
03193      
03194      /* Populate the return buffers with the error data */
03195      for (unsigned int i = 0, k = 1; i < (unsigned int)num_errors && (error_type_buffer || error_num_buffer); i++) {
03196        
03197        /* Check if the error type has been requested */
03198        if (error_type_buffer) {
03199          error_type_buffer[i] = payload_buffer[k];
03200        }
03201        k++;
03202        
03203        /* Check if the error number has been requested */
03204        if (error_num_buffer) {
03205          error_num_buffer[i] = payload_buffer[k];
03206        }
03207        k++;
03208        
03209      }
03210 
03211   }
03212   
03216   void SickLMS2xx::_getSickStatus( ) throw( SickTimeoutException, SickIOException, SickThreadException ) {
03217 
03218     SickLMS2xxMessage message,response;
03219 
03220     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
03221 
03222     /* The command to request LMS status */
03223     payload_buffer[0] = 0x31;
03224 
03225     /* Build the request message */
03226     message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,1);
03227 
03228     try {
03229     
03230       /* Send the status request and get a reply */
03231       _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
03232 
03233     }
03234     
03235     /* Catch any timeout exceptions */
03236     catch(SickTimeoutException &sick_timeout_exception) {
03237       std::cerr << sick_timeout_exception.what() << std::endl;
03238       throw;
03239     }
03240     
03241     /* Catch any I/O exceptions */
03242     catch(SickIOException &sick_io_exception) {
03243       std::cerr << sick_io_exception.what() << std::endl;
03244       throw;
03245     }
03246     
03247     /* Catch any thread exceptions */
03248     catch(SickThreadException &sick_thread_exception) {
03249       std::cerr << sick_thread_exception.what() << std::endl;
03250       throw;
03251     }
03252     
03253     /* Catch anything else */
03254     catch(...) {
03255       std::cerr << "SickLMS2xx::_getSickStatus: Unknown exception!" << std::endl;
03256       throw;
03257     }
03258 
03259     /* Reset the payload buffer */
03260     payload_buffer[0] = 0;
03261 
03262     /* Extract the payload contents */
03263     response.GetPayload(payload_buffer);
03264     
03265     /*
03266      * Extract the current Sick LMS operating config
03267      */
03268 
03269     /* Buffer the Sick LMS operating mode */
03270     _sick_operating_status.sick_operating_mode = payload_buffer[8];
03271     
03272     /* Buffer the status code */
03273     _sick_operating_status.sick_device_status = (payload_buffer[9]) ? SICK_STATUS_ERROR : SICK_STATUS_OK;
03274     
03275     /* Buffer the number of motor revolutions */
03276     memcpy(&_sick_operating_status.sick_num_motor_revs,&payload_buffer[67],2);
03277     _sick_operating_status.sick_num_motor_revs = sick_lms_2xx_to_host_byte_order(_sick_operating_status.sick_num_motor_revs);
03278     
03279     /* Buffer the measuring mode of the device */
03280     _sick_operating_status.sick_measuring_mode = payload_buffer[102];
03281     
03282     /* Buffer the scan angle of the device */
03283     memcpy(&_sick_operating_status.sick_scan_angle,&payload_buffer[107],2);
03284     _sick_operating_status.sick_scan_angle =
03285       sick_lms_2xx_to_host_byte_order(_sick_operating_status.sick_scan_angle);
03286     
03287     /* Buffer the angular resolution of the device */
03288     memcpy(&_sick_operating_status.sick_scan_resolution,&payload_buffer[109],2);
03289     _sick_operating_status.sick_scan_resolution =
03290       sick_lms_2xx_to_host_byte_order(_sick_operating_status.sick_scan_resolution);
03291 
03292     /* Buffer the variant type */
03293     _sick_operating_status.sick_variant = payload_buffer[18];
03294     
03295     /* Buffer the Sick LMS address */
03296     _sick_operating_status.sick_address = payload_buffer[120];
03297     
03298     /* Buffer the current measured value unit */
03299     _sick_operating_status.sick_measuring_units = payload_buffer[122];
03300     
03301     /* Buffer the laser switch flag */
03302     _sick_operating_status.sick_laser_mode = payload_buffer[123];
03303 
03304     
03305     /*
03306      * Extract the current Sick LMS software config
03307      */
03308     
03309     /* Buffer the software version string */
03310     memcpy(_sick_software_status.sick_system_software_version,&payload_buffer[1],7);
03311 
03312     /* Buffer the boot prom software version */
03313     memcpy(_sick_software_status.sick_prom_software_version,&payload_buffer[124],7);
03314 
03315     /*
03316      * Extract the Sick LMS restart config
03317      */
03318 
03319     /* Buffer the restart mode of the device */
03320     _sick_restart_status.sick_restart_mode = payload_buffer[111];
03321     
03322     /* Buffer the restart time of the device */
03323     memcpy(&_sick_restart_status.sick_restart_time,&payload_buffer[112],2);
03324     _sick_restart_status.sick_restart_time =
03325       sick_lms_2xx_to_host_byte_order(_sick_restart_status.sick_restart_time);
03326     
03327     /*
03328      * Extract the Sick LMS pollution status
03329      */
03330 
03331     /* Buffer the pollution values */
03332     for (unsigned int i = 0, k = 19; i < 8; i++, k+=2) {
03333       memcpy(&_sick_pollution_status.sick_pollution_vals[i],&payload_buffer[k],2);
03334       _sick_pollution_status.sick_pollution_vals[i] =
03335         sick_lms_2xx_to_host_byte_order(_sick_pollution_status.sick_pollution_vals[i]);
03336     }
03337 
03338     /* Buffer the reference pollution values */
03339     for (unsigned int i = 0, k = 35; i < 4; i++, k+=2) {
03340       memcpy(&_sick_pollution_status.sick_reference_pollution_vals[i],&payload_buffer[k],2);
03341       _sick_pollution_status.sick_reference_pollution_vals[i] =
03342         sick_lms_2xx_to_host_byte_order(_sick_pollution_status.sick_reference_pollution_vals[i]);
03343     }
03344     
03345     /* Buffer the calibrating pollution values */
03346     for (unsigned int i = 0, k = 43; i < 8; i++, k+=2) {
03347       memcpy(&_sick_pollution_status.sick_pollution_calibration_vals[i],&payload_buffer[k],2);
03348       _sick_pollution_status.sick_pollution_calibration_vals[i] =
03349         sick_lms_2xx_to_host_byte_order(_sick_pollution_status.sick_pollution_calibration_vals[i]);
03350     }
03351 
03352     /* Buffer the calibrating reference pollution values */
03353     for (unsigned int i = 0, k = 59; i < 4; i++, k+=2) {
03354       memcpy(&_sick_pollution_status.sick_reference_pollution_calibration_vals[i],&payload_buffer[k],2);
03355       _sick_pollution_status.sick_reference_pollution_calibration_vals[i] =
03356         sick_lms_2xx_to_host_byte_order(_sick_pollution_status.sick_reference_pollution_calibration_vals[i]);
03357     }
03358 
03359     /*
03360      * Extract the Sick LMS signal config 
03361      */
03362     
03363     /* Buffer the reference scale 1 value (Dark signal 100%) */
03364     memcpy(&_sick_signal_status.sick_reference_scale_1_dark_100,&payload_buffer[71],2);
03365     _sick_signal_status.sick_reference_scale_1_dark_100 =
03366       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_reference_scale_1_dark_100);
03367 
03368     /* Buffer the reference scale 2 value (Dark signal 100%) */
03369     memcpy(&_sick_signal_status.sick_reference_scale_2_dark_100,&payload_buffer[75],2);
03370     _sick_signal_status.sick_reference_scale_2_dark_100 =
03371       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_reference_scale_2_dark_100);
03372 
03373     /* Buffer the reference scale 1 value (Dark signal 66%) */
03374     memcpy(&_sick_signal_status.sick_reference_scale_1_dark_66,&payload_buffer[77],2);
03375     _sick_signal_status.sick_reference_scale_1_dark_66 =
03376       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_reference_scale_1_dark_66);
03377 
03378     /* Buffer the reference scale 2 value (Dark signal 100%) */
03379     memcpy(&_sick_signal_status.sick_reference_scale_2_dark_66,&payload_buffer[81],2);
03380     _sick_signal_status.sick_reference_scale_2_dark_66 =
03381       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_reference_scale_2_dark_66);
03382 
03383     /* Buffer the signal amplitude */
03384     memcpy(&_sick_signal_status.sick_signal_amplitude,&payload_buffer[83],2);
03385     _sick_signal_status.sick_signal_amplitude =
03386       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_signal_amplitude);
03387 
03388     /* Buffer the angle used for power measurement */
03389     memcpy(&_sick_signal_status.sick_current_angle,&payload_buffer[85],2);
03390     _sick_signal_status.sick_current_angle =
03391       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_current_angle);
03392 
03393     /* Buffer the peak threshold value */
03394     memcpy(&_sick_signal_status.sick_peak_threshold,&payload_buffer[87],2);
03395     _sick_signal_status.sick_peak_threshold =
03396       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_peak_threshold);
03397     
03398     /* Buffer the angle used for reference target power measurement */
03399     memcpy(&_sick_signal_status.sick_angle_of_measurement,&payload_buffer[89],2);
03400     _sick_signal_status.sick_angle_of_measurement =
03401       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_angle_of_measurement);
03402 
03403     /* Buffer the signal amplitude calibration value */
03404     memcpy(&_sick_signal_status.sick_signal_amplitude_calibration_val,&payload_buffer[91],2);
03405     _sick_signal_status.sick_signal_amplitude_calibration_val =
03406       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_signal_amplitude_calibration_val);
03407 
03408     /* Buffer the target value of stop threshold */
03409     memcpy(&_sick_signal_status.sick_stop_threshold_target_value,&payload_buffer[93],2);
03410     _sick_signal_status.sick_stop_threshold_target_value =
03411       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_stop_threshold_target_value);
03412     
03413     /* Buffer the target value of peak threshold */
03414     memcpy(&_sick_signal_status.sick_peak_threshold_target_value,&payload_buffer[95],2);
03415     _sick_signal_status.sick_peak_threshold_target_value =
03416       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_peak_threshold_target_value);
03417     
03418     /* Buffer the actual value of stop threshold */
03419     memcpy(&_sick_signal_status.sick_stop_threshold_actual_value,&payload_buffer[97],2);
03420     _sick_signal_status.sick_stop_threshold_actual_value =
03421       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_stop_threshold_actual_value);
03422 
03423     /* Buffer the actual value of peak threshold */
03424     memcpy(&_sick_signal_status.sick_peak_threshold_actual_value,&payload_buffer[99],2);
03425     _sick_signal_status.sick_peak_threshold_actual_value =
03426       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_peak_threshold_actual_value);
03427 
03428     /* Buffer reference target "single measured values" */
03429     memcpy(&_sick_signal_status.sick_reference_target_single_measured_vals,&payload_buffer[103],2);
03430     _sick_signal_status.sick_reference_target_single_measured_vals =
03431       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_reference_target_single_measured_vals);
03432   
03433     /* Buffer reference target "mean measured values" */
03434     memcpy(&_sick_signal_status.sick_reference_target_mean_measured_vals,&payload_buffer[105],2);
03435     _sick_signal_status.sick_reference_target_mean_measured_vals =
03436       sick_lms_2xx_to_host_byte_order(_sick_signal_status.sick_reference_target_mean_measured_vals);
03437 
03438 
03439     /*
03440      * Extract the Sick LMS field config
03441      */
03442 
03443     /* Buffer the offset for multiple evaluations of field set 2 */
03444     _sick_field_status.sick_multiple_evaluation_offset_field_2 = payload_buffer[114];
03445 
03446     /* Buffer the evaluation number */
03447     _sick_field_status.sick_field_evaluation_number = payload_buffer[118];
03448 
03449     /* Buffer the active field set number */
03450     _sick_field_status.sick_field_set_number = payload_buffer[121];
03451 
03452 
03453     /*
03454      * Extract the Sick LMS baud config
03455      */
03456     
03457     /* Buffer the permanent baud rate flag */
03458     _sick_baud_status.sick_permanent_baud_rate = payload_buffer[119];
03459     
03460     /* Buffer the baud rate of the device */
03461     memcpy(&_sick_baud_status.sick_baud_rate,&payload_buffer[116],2);
03462     _sick_baud_status.sick_baud_rate =
03463       sick_lms_2xx_to_host_byte_order(_sick_baud_status.sick_baud_rate);
03464 
03465     /* Buffer calibration value 1 for counter 0 */
03466     //memcpy(&_sick_status_data.sick_calibration_counter_0_value_1,&payload_buffer[131],4);
03467     //_sick_status_data.sick_calibration_counter_0_value_1 =
03468     //  sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_calibration_counter_0_value_1);
03469 
03470     /* Buffer calibration value 2 for counter 0 */
03471     //memcpy(&_sick_status_data.sick_calibration_counter_0_value_2,&payload_buffer[135],4);
03472     //_sick_status_data.sick_calibration_counter_0_value_2 =
03473     //  sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_calibration_counter_0_value_2);
03474 
03475     /* Buffer calibration value 1 for counter 1 */
03476     //memcpy(&_sick_status_data.sick_calibration_counter_1_value_1,&payload_buffer[139],4);
03477     //_sick_status_data.sick_calibration_counter_1_value_1 =
03478     //  sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_calibration_counter_1_value_1);
03479 
03480     /* Buffer calibration value 2 for counter 1 */
03481     //memcpy(&_sick_status_data.sick_calibration_counter_1_value_2,&payload_buffer[143],4);
03482     //_sick_status_data.sick_calibration_counter_1_value_2 =
03483     //  sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_calibration_counter_1_value_2);
03484 
03485     /* Buffer M0 value counter 0 */
03486     //memcpy(&_sick_status_data.sick_counter_0_M0,&payload_buffer[147],2);
03487     //_sick_status_data.sick_counter_0_M0 = sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_counter_0_M0);
03488 
03489     /* Buffer M0 value counter 1 */
03490     //memcpy(&_sick_status_data.sick_counter_1_M0,&payload_buffer[149],2);
03491     //_sick_status_data.sick_counter_1_M0 = sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_counter_1_M0);
03492 
03493     /* Buffer calibration interval */
03494     //memcpy(&_sick_status_data.sick_calibration_interval,&payload_buffer[151],2);
03495     //_sick_status_data.sick_calibration_interval = sick_lms_2xx_to_host_byte_order(_sick_status_data.sick_calibration_interval);
03496   
03497   }
03498 
03502   void SickLMS2xx::_setSickOpModeInstallation( )
03503     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03504     
03505     /* Assign the password for entering installation mode */
03506     uint8_t sick_password[9] = DEFAULT_SICK_LMS_2XX_SICK_PASSWORD;
03507 
03508     /* Check if mode should be changed */
03509     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_INSTALLATION) {
03510 
03511       try {
03512 
03513         /* Attempt to switch modes! */  
03514         _switchSickOperatingMode(SICK_OP_MODE_INSTALLATION,sick_password);
03515 
03516       }
03517 
03518       /* Catch any config exceptions */
03519       catch(SickConfigException &sick_config_exception) {
03520         std::cerr << sick_config_exception.what() << std::endl;
03521         throw;
03522       }
03523       
03524       /* Catch any timeout exceptions */
03525       catch(SickTimeoutException &sick_timeout_exception) {
03526         std::cerr << sick_timeout_exception.what() << std::endl;
03527         throw;
03528       }
03529       
03530       /* Catch any I/O exceptions */
03531       catch(SickIOException &sick_io_exception) {
03532         std::cerr << sick_io_exception.what() << std::endl;
03533         throw;
03534       }
03535       
03536       /* Catch any thread exceptions */
03537       catch(SickThreadException &sick_thread_exception) {
03538         std::cerr << sick_thread_exception.what() << std::endl;
03539         throw;
03540       }
03541       
03542       /* Catch anything else */
03543       catch(...) {
03544         std::cerr << "SickLMS2xx::_setSickOpModeInstallation: Unknown exception!!!" << std::endl;
03545         throw;
03546       } 
03547       
03548       /* Assign the new operating mode */
03549       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_INSTALLATION;
03550 
03551       /* Reset these parameters */
03552       _sick_mean_value_sample_size = _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03553 
03554     }
03555 
03556   }
03557 
03561   void SickLMS2xx::_setSickOpModeDiagnostic( )
03562     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03563 
03564     /* Check if mode should be changed */
03565     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_DIAGNOSTIC) {
03566 
03567       std::cout << "\tAttempting to enter diagnostic mode..." << std::endl;
03568       
03569       try {
03570 
03571         /* Attempt to switch modes! */  
03572         _switchSickOperatingMode(SICK_OP_MODE_DIAGNOSTIC);
03573 
03574       }
03575 
03576       /* Catch any config exceptions */
03577       catch(SickConfigException &sick_config_exception) {
03578         std::cerr << sick_config_exception.what() << std::endl;
03579         throw;
03580       }
03581       
03582       /* Catch any timeout exceptions */
03583       catch(SickTimeoutException &sick_timeout_exception) {
03584         std::cerr << sick_timeout_exception.what() << std::endl;
03585         throw;
03586       }
03587       
03588       /* Catch any I/O exceptions */
03589       catch(SickIOException &sick_io_exception) {
03590         std::cerr << sick_io_exception.what() << std::endl;
03591         throw;
03592       }
03593       
03594       /* Catch any thread exceptions */
03595       catch(SickThreadException &sick_thread_exception) {
03596         std::cerr << sick_thread_exception.what() << std::endl;
03597         throw;
03598       }
03599       
03600       /* Catch anything else */
03601       catch(...) {
03602         std::cerr << "SickLMS2xx::_setSickOpModeInstallation: Unknown exception!!!" << std::endl;
03603         throw;
03604       } 
03605       
03606       /* Assign the new operating mode */
03607       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_DIAGNOSTIC;
03608 
03609       /* Reset these parameters */
03610       _sick_mean_value_sample_size = _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03611 
03612       std::cout << "Success!" << std::endl;
03613       
03614     }
03615 
03616   }
03617 
03621   void SickLMS2xx::_setSickOpModeMonitorRequestValues( )
03622     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03623 
03624     /* Check if mode should be changed */
03625     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_REQUEST_VALUES) {
03626 
03627       try {
03628 
03629         /* Attempt to switch operating mode */
03630         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_REQUEST_VALUES);
03631 
03632       }
03633 
03634       /* Catch any config exceptions */
03635       catch(SickConfigException &sick_config_exception) {
03636         std::cerr << sick_config_exception.what() << std::endl;
03637         throw;
03638       }
03639       
03640       /* Catch any timeout exceptions */
03641       catch(SickTimeoutException &sick_timeout_exception) {
03642         std::cerr << sick_timeout_exception.what() << std::endl;
03643         throw;
03644       }
03645       
03646       /* Catch any I/O exceptions */
03647       catch(SickIOException &sick_io_exception) {
03648         std::cerr << sick_io_exception.what() << std::endl;
03649         throw;
03650       }
03651       
03652       /* Catch any thread exceptions */
03653       catch(SickThreadException &sick_thread_exception) {
03654         std::cerr << sick_thread_exception.what() << std::endl;
03655         throw;
03656       }
03657       
03658       /* Catch anything else */
03659       catch(...) {
03660         std::cerr << "SickLMS2xx::_setSickOpModeMonitorRequestValues: Unknown exception!!!" << std::endl;
03661         throw;
03662       } 
03663       
03664       /* Assign the new operating mode */
03665       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_REQUEST_VALUES;
03666 
03667       /* Reset these parameters */
03668       _sick_mean_value_sample_size = _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03669 
03670     }
03671 
03672   }
03673 
03677   void SickLMS2xx::_setSickOpModeMonitorStreamValues( )
03678     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03679 
03680     /* Check if mode should be changed */
03681     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_VALUES) {
03682 
03683       std::cout << "\tRequesting measured value data stream..." << std::endl;
03684       
03685       try {
03686 
03687         /* Attempt to switch modes */
03688         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_VALUES);
03689 
03690       }
03691 
03692       /* Catch any config exceptions */
03693       catch(SickConfigException &sick_config_exception) {
03694         std::cerr << sick_config_exception.what() << std::endl;
03695         throw;
03696       }
03697       
03698       /* Catch any timeout exceptions */
03699       catch(SickTimeoutException &sick_timeout_exception) {
03700         std::cerr << sick_timeout_exception.what() << std::endl;
03701         throw;
03702       }
03703       
03704       /* Catch any I/O exceptions */
03705       catch(SickIOException &sick_io_exception) {
03706         std::cerr << sick_io_exception.what() << std::endl;
03707         throw;
03708       }
03709       
03710       /* Catch any thread exceptions */
03711       catch(SickThreadException &sick_thread_exception) {
03712         std::cerr << sick_thread_exception.what() << std::endl;
03713         throw;
03714       }
03715       
03716       /* Catch anything else */
03717       catch(...) {
03718         std::cerr << "SickLMS2xx::_setSickOpModeMonitorStreamValues: Unknown exception!!!" << std::endl;
03719         throw;
03720       } 
03721       
03722       /* Assign the new operating mode */
03723       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_VALUES;
03724 
03725       /* Reset these parameters */
03726       _sick_mean_value_sample_size = _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03727 
03728       std::cout << "\t\tData stream started!" << std::endl;
03729       
03730     }
03731 
03732   }
03733 
03737   void SickLMS2xx::_setSickOpModeMonitorStreamRangeAndReflectivity( )
03738     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03739 
03740     /* A sanity check to make sure that the command is supported */
03741     if (_sick_type != SICK_LMS_TYPE_211_S14 && _sick_type != SICK_LMS_TYPE_221_S14 && _sick_type != SICK_LMS_TYPE_291_S14) {
03742       throw SickConfigException("SickLMS2xx::_setSickOpModeMonitorStreamRangeAndReflectivity: Mode not supported by this model!");
03743     }
03744     
03745     /* Check if mode should be changed */
03746     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT) {
03747 
03748       /* Define the parameters */
03749       uint8_t mode_params[4] = {0x01,0x00,0xB5,0x00}; //1 to 181
03750 
03751       std::cout << "\tRequesting range & reflectivity data stream..." << std::endl;
03752       
03753       try {
03754 
03755         /* Attempt to switch modes */
03756         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT,mode_params);
03757 
03758       }
03759       
03760       /* Catch any config exceptions */
03761       catch(SickConfigException &sick_config_exception) {
03762         std::cerr << sick_config_exception.what() << std::endl;
03763         throw;
03764       }
03765       
03766       /* Catch any timeout exceptions */
03767       catch(SickTimeoutException &sick_timeout_exception) {
03768         std::cerr << sick_timeout_exception.what() << std::endl;
03769         throw;
03770       }
03771       
03772       /* Catch any I/O exceptions */
03773       catch(SickIOException &sick_io_exception) {
03774         std::cerr << sick_io_exception.what() << std::endl;
03775         throw;
03776       }
03777       
03778       /* Catch any thread exceptions */
03779       catch(SickThreadException &sick_thread_exception) {
03780         std::cerr << sick_thread_exception.what() << std::endl;
03781         throw;
03782       }
03783       
03784       /* Catch anything else */
03785       catch(...) {
03786         std::cerr << "SickLMS2xx::_setSickOpModeStreamRangeAndReflectivity: Unknown exception!!!" << std::endl;
03787         throw;
03788       } 
03789       
03790       /* Assign the new operating mode */
03791       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT;
03792 
03793       /* Reset these parameters */
03794       _sick_mean_value_sample_size = _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03795 
03796       std::cout << "\t\tData stream started!" << std::endl;
03797       
03798     }
03799 
03800   }
03801 
03805   void SickLMS2xx::_setSickOpModeMonitorStreamValuesFromPartialScan( )
03806     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03807 
03808     /* Check if mode should be changed */
03809     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN) {
03810 
03811       std::cout << "\tRequesting partial scan data stream..." << std::endl;
03812       
03813       try {
03814 
03815         /* Attempt to switch modes */
03816         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN);
03817 
03818       }
03819 
03820       /* Catch any config exceptions */
03821       catch(SickConfigException &sick_config_exception) {
03822         std::cerr << sick_config_exception.what() << std::endl;
03823         throw;
03824       }
03825       
03826       /* Catch any timeout exceptions */
03827       catch(SickTimeoutException &sick_timeout_exception) {
03828         std::cerr << sick_timeout_exception.what() << std::endl;
03829         throw;
03830       }
03831       
03832       /* Catch any I/O exceptions */
03833       catch(SickIOException &sick_io_exception) {
03834         std::cerr << sick_io_exception.what() << std::endl;
03835         throw;
03836       }
03837       
03838       /* Catch any thread exceptions */
03839       catch(SickThreadException &sick_thread_exception) {
03840         std::cerr << sick_thread_exception.what() << std::endl;
03841         throw;
03842       }
03843       
03844       /* Catch anything else */
03845       catch(...) {
03846         std::cerr << "SickLMS2xx::_setSickOpModeStreamValuesFromPartialScan: Unknown exception!!!" << std::endl;
03847         throw;
03848       } 
03849       
03850       /* Assign the new operating mode */
03851       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN;
03852 
03853       /* Reset these parameters */
03854       _sick_mean_value_sample_size = _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03855 
03856       std::cout << "\t\tData stream started!" << std::endl;
03857       
03858     }
03859 
03860   }
03861 
03865   void SickLMS2xx::_setSickOpModeMonitorStreamMeanValues( const uint8_t sample_size )
03866     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03867 
03868     /* Check if mode should be changed */
03869     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES ||
03870         _sick_mean_value_sample_size != sample_size) {
03871 
03872       /* Make sure the sample size is legitimate */
03873       if(sample_size < 2 || sample_size > 250) {
03874         throw SickConfigException("SickLMS2xx::_setSickOpModeMonitorStreamMeanValues: Invalid sample size!");
03875       }
03876 
03877       std::cout << "\tRequesting mean value data stream (sample size = " << (int)sample_size << ")..." << std::endl;
03878       
03879       try {
03880 
03881         /* Attempt to switch modes */
03882         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES,&sample_size);
03883 
03884       }
03885 
03886       /* Catch any config exceptions */
03887       catch(SickConfigException &sick_config_exception) {
03888         std::cerr << sick_config_exception.what() << std::endl;
03889         throw;
03890       }
03891       
03892       /* Catch any timeout exceptions */
03893       catch(SickTimeoutException &sick_timeout_exception) {
03894         std::cerr << sick_timeout_exception.what() << std::endl;
03895         throw;
03896       }
03897       
03898       /* Catch any I/O exceptions */
03899       catch(SickIOException &sick_io_exception) {
03900         std::cerr << sick_io_exception.what() << std::endl;
03901         throw;
03902       }
03903       
03904       /* Catch any thread exceptions */
03905       catch(SickThreadException &sick_thread_exception) {
03906         std::cerr << sick_thread_exception.what() << std::endl;
03907         throw;
03908       }
03909       
03910       /* Catch anything else */
03911       catch(...) {
03912         std::cerr << "SickLMS2xx::_setSickOpModeStreamRangeFromPartialScan: Unknown exception!!!" << std::endl;
03913         throw;
03914       } 
03915       
03916       /* Assign the new operating mode */
03917       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES;
03918 
03919       /* Buffer the current sample size! */
03920       _sick_mean_value_sample_size = sample_size;
03921 
03922       /* Reset these parameters */
03923       _sick_values_subrange_start_index = _sick_values_subrange_stop_index = 0;
03924       
03925       std::cout << "\t\tData stream started!" << std::endl;
03926       
03927     }
03928 
03929   }
03930 
03936   void SickLMS2xx::_setSickOpModeMonitorStreamValuesSubrange( const uint16_t subrange_start_index, const uint16_t subrange_stop_index )
03937     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
03938 
03939     /* Check if mode should be changed */
03940     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE ||
03941         _sick_values_subrange_start_index != subrange_start_index ||
03942         _sick_values_subrange_stop_index != subrange_stop_index ) {
03943 
03944       /* Compute the maximum subregion bound */
03945       unsigned int max_subrange_stop_index = (unsigned int)((_sick_operating_status.sick_scan_angle*100)/_sick_operating_status.sick_scan_resolution + 1) ;
03946       
03947       /* Ensure the subregion is properly defined for the given variant */
03948       if(subrange_start_index > subrange_stop_index || subrange_start_index == 0 || subrange_stop_index > max_subrange_stop_index) {
03949         throw SickConfigException("SickLMS2xx::_setSickOpMonitorStreamValuesSubrange: Invalid subregion bounds!");
03950       }
03951       
03952       /* Setup a few buffers */
03953       uint8_t mode_params[4] = {0};
03954       uint16_t temp_buffer = 0;
03955 
03956       /* Assign the subrange start index */
03957       temp_buffer = host_to_sick_lms_2xx_byte_order(subrange_start_index);
03958       memcpy(mode_params,&temp_buffer,2);
03959 
03960       /* Assign the subrange stop index */
03961       temp_buffer = host_to_sick_lms_2xx_byte_order(subrange_stop_index);
03962       memcpy(&mode_params[2],&temp_buffer,2);
03963       
03964       std::cout << "\tRequesting measured value stream... (subrange = [" << subrange_start_index << "," << subrange_stop_index << "])" << std::endl;
03965       
03966       try {
03967 
03968         /* Attempt to switch modes */
03969         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE,mode_params);
03970 
03971       }
03972       
03973       /* Catch any config exceptions */
03974       catch(SickConfigException &sick_config_exception) {
03975         std::cerr << sick_config_exception.what() << std::endl;
03976         throw;
03977       }
03978       
03979       /* Catch any timeout exceptions */
03980       catch(SickTimeoutException &sick_timeout_exception) {
03981         std::cerr << sick_timeout_exception.what() << std::endl;
03982         throw;
03983       }
03984       
03985       /* Catch any I/O exceptions */
03986       catch(SickIOException &sick_io_exception) {
03987         std::cerr << sick_io_exception.what() << std::endl;
03988         throw;
03989       }
03990       
03991       /* Catch any thread exceptions */
03992       catch(SickThreadException &sick_thread_exception) {
03993         std::cerr << sick_thread_exception.what() << std::endl;
03994         throw;
03995       }
03996       
03997       /* Catch anything else */
03998       catch(...) {
03999         std::cerr << "SickLMS2xx::_setSickOpModeInstallation: Unknown exception!!!" << std::endl;
04000         throw;
04001       } 
04002       
04003       /* Assign the new operating mode */
04004       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE;
04005 
04006       /* Reset this parameter */
04007       _sick_mean_value_sample_size = 0;
04008 
04009       /* Buffer the starting/stopping indices */
04010       _sick_values_subrange_start_index = subrange_start_index;
04011       _sick_values_subrange_stop_index = subrange_stop_index;
04012       
04013       std::cout << "\t\tData stream started!" << std::endl;
04014 
04015     }
04016 
04017   }
04018 
04025   void SickLMS2xx::_setSickOpModeMonitorStreamMeanValuesSubrange( const uint16_t sample_size, const uint16_t subrange_start_index, const uint16_t subrange_stop_index )
04026     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
04027 
04028     /* Check if mode should be changed */
04029     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE ||
04030         _sick_values_subrange_start_index != subrange_start_index ||
04031         _sick_values_subrange_stop_index != subrange_stop_index ||
04032         _sick_mean_value_sample_size != sample_size ) {
04033 
04034       /* Make sure the sample size is legit */
04035       if(sample_size < 2 || sample_size > 250) {
04036         throw SickConfigException("SickLMS2xx::_setSickOpModeMonitorStreamMeanValuesSubrange: Invalid sample size!");
04037       }
04038       
04039       /* Compute the maximum subregion bound */
04040       unsigned int max_subrange_stop_index = (unsigned int)((_sick_operating_status.sick_scan_angle*100)/_sick_operating_status.sick_scan_resolution + 1) ;
04041       
04042       /* Ensure the subregion is properly defined for the given variant */
04043       if(subrange_start_index > subrange_stop_index || subrange_start_index == 0 || subrange_stop_index > max_subrange_stop_index) {
04044         throw SickConfigException("SickLMS2xx::_setSickOpMonitorStreamMeanValuesSubrange: Invalid subregion bounds!");
04045       }
04046       
04047       /* Setup a few buffers */
04048       uint8_t mode_params[5] = {0};
04049       uint16_t temp_buffer = 0;
04050 
04051       /* Assign the sample size */
04052       mode_params[0] = sample_size;
04053 
04054       /* Assign the subrange start index */
04055       temp_buffer = host_to_sick_lms_2xx_byte_order(subrange_start_index);
04056       memcpy(&mode_params[1],&temp_buffer,2);
04057 
04058       /* Assign the subrange stop index */
04059       temp_buffer = host_to_sick_lms_2xx_byte_order(subrange_stop_index);
04060       memcpy(&mode_params[3],&temp_buffer,2);
04061       
04062       std::cout << "\tRequesting mean value stream... (subrange = [" << subrange_start_index << "," << subrange_stop_index << "])" << std::endl;
04063       
04064       try {
04065 
04066         /* Attempt to switch modes */
04067         _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE,mode_params);
04068 
04069       }
04070       
04071       /* Catch any config exceptions */
04072       catch(SickConfigException &sick_config_exception) {
04073         std::cerr << sick_config_exception.what() << std::endl;
04074         throw;
04075       }
04076       
04077       /* Catch any timeout exceptions */
04078       catch(SickTimeoutException &sick_timeout_exception) {
04079         std::cerr << sick_timeout_exception.what() << std::endl;
04080         throw;
04081       }
04082       
04083       /* Catch any I/O exceptions */
04084       catch(SickIOException &sick_io_exception) {
04085         std::cerr << sick_io_exception.what() << std::endl;
04086         throw;
04087       }
04088       
04089       /* Catch any thread exceptions */
04090       catch(SickThreadException &sick_thread_exception) {
04091         std::cerr << sick_thread_exception.what() << std::endl;
04092         throw;
04093       }
04094       
04095       /* Catch anything else */
04096       catch(...) {
04097         std::cerr << "SickLMS2xx::_setSickOpModeInstallation: Unknown exception!!!" << std::endl;
04098         throw;
04099       } 
04100       
04101       /* Assign the new operating mode */
04102       _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE;
04103 
04104       /* Buffer the sample size */
04105       _sick_mean_value_sample_size = sample_size;
04106 
04107       /* Buffer the starting/stopping indices */
04108       _sick_values_subrange_start_index = subrange_start_index;
04109       _sick_values_subrange_stop_index = subrange_stop_index;
04110       
04111       std::cout << "\t\tData stream started!" << std::endl;
04112 
04113     }
04114 
04115   }
04116   
04122   void SickLMS2xx::_switchSickOperatingMode( const uint8_t sick_mode, const uint8_t * const mode_params )
04123     throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException) {
04124 
04125     SickLMS2xxMessage message,response;
04126 
04127     uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};    
04128     uint16_t num_partial_scans = 0;
04129 
04130     /* Construct the correct switch mode packet */
04131     payload_buffer[0] = 0x20;
04132     payload_buffer[1] = sick_mode;
04133 
04134     switch(sick_mode) {
04135    
04136     case SICK_OP_MODE_INSTALLATION:
04137 
04138       /* Make sure the params are defined */
04139       if(mode_params == NULL) {
04140         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04141       }
04142 
04143       memcpy(&payload_buffer[2],mode_params,8); //Copy password
04144       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,10);
04145       break;
04146 
04147     case SICK_OP_MODE_DIAGNOSTIC:
04148       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04149       break;
04150 
04151     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUE_FOR_EACH_SEGMENT:
04152       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04153       break;
04154 
04155     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VALUE_ON_OBJECT:
04156       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04157       break;
04158 
04159     case SICK_OP_MODE_MONITOR_STREAM_MIN_VERT_DIST_TO_OBJECT:
04160       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04161       break;
04162 
04163     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VERT_DIST_TO_OBJECT:
04164       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04165       break;
04166 
04167     case SICK_OP_MODE_MONITOR_STREAM_VALUES:
04168       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04169       break;
04170 
04171     case SICK_OP_MODE_MONITOR_REQUEST_VALUES:
04172       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04173       break;
04174 
04175     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES:
04176 
04177       /* Make sure the params are defined */
04178       if(mode_params == NULL) {
04179         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04180       }
04181 
04182       payload_buffer[2] = *mode_params;
04183       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,3);
04184       break;
04185 
04186     case SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE:
04187 
04188       /* Make sure the params are defined */
04189       if(mode_params == NULL) {
04190         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04191       }
04192 
04193       memcpy(&payload_buffer[2],mode_params,2);       //Begin range
04194       memcpy(&payload_buffer[4],&mode_params[2],2);   //End range
04195       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,6);
04196       break;
04197 
04198     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE:
04199 
04200       /* Make sure the params are defined */
04201       if(mode_params == NULL) {
04202         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04203       }
04204 
04205       payload_buffer[2] = mode_params[0];             //Sample size 
04206       memcpy(&payload_buffer[3],&mode_params[1],2);   //Begin mean range
04207       memcpy(&payload_buffer[5],&mode_params[3],2);   //End mean range
04208       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,7);
04209       break;
04210 
04211     case SICK_OP_MODE_MONITOR_STREAM_VALUES_WITH_FIELDS:
04212 
04213       /* Make sure the params are defined */
04214       if(mode_params == NULL) {
04215         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04216       }
04217 
04218       memcpy(&payload_buffer[2],mode_params,2);       //Start
04219       memcpy(&payload_buffer[4],&mode_params[2],2);   //End
04220       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,6);
04221       break;
04222 
04223     case SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN:
04224       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04225       break;
04226 
04227     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT_FROM_PARTIAL_SCAN:
04228 
04229       /* Make sure the params are defined */
04230       if(mode_params == NULL) {
04231         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04232       }
04233 
04234       /* Get the number of partial scans (between 1 and 5) */
04235       memcpy(&num_partial_scans,mode_params,2);
04236 
04237       /* Setup the command packet */
04238       memcpy(&payload_buffer[2],mode_params,num_partial_scans*4+2);
04239       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,num_partial_scans*4+4);
04240       break;
04241 
04242     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUES_FOR_EACH_SEGMENT_SUBRANGE:
04243 
04244       /* Make sure the params are defined */
04245       if(mode_params == NULL) {
04246         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04247       }
04248     
04249       /* Get the number of partial scans (between 1 and 5) */
04250       memcpy(&num_partial_scans,mode_params,2);
04251     
04252       /* Setup the command packet */
04253       memcpy(&payload_buffer[2],mode_params,num_partial_scans*4+2);    
04254       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,num_partial_scans*4+4);
04255       break;
04256 
04257     case SICK_OP_MODE_MONITOR_NAVIGATION:
04258       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,2);
04259       break;
04260 
04261     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT:
04262 
04263       /* Make sure the params are defined */
04264       if(mode_params == NULL) {
04265         throw SickConfigException("SickLMS2xx::_switchSickOperatingMode - Requested mode requires parameters!");
04266       }
04267       
04268       memcpy(&payload_buffer[2],mode_params,2);       //Start
04269       memcpy(&payload_buffer[4],&mode_params[2],2);   //End
04270       message.BuildMessage(DEFAULT_SICK_LMS_2XX_SICK_ADDRESS,payload_buffer,6);
04271       break;
04272 
04273     case SICK_OP_MODE_UNKNOWN:
04274       //Let this case go straight to default
04275     
04276     default:
04277       throw SickConfigException("SickLMS2xx::_switchSickOperatingMode: Unrecognized operating mode!");
04278     }
04279 
04280     try {
04281 
04282       /* Attempt to send the message and get the reply */
04283       _sendMessageAndGetReply(message,response,DEFAULT_SICK_LMS_2XX_SICK_SWITCH_MODE_TIMEOUT,DEFAULT_SICK_LMS_2XX_NUM_TRIES);
04284       
04285     }
04286 
04287     /* Catch any timeout exceptions */
04288     catch(SickTimeoutException &sick_timeout_exception) {
04289       std::cerr << sick_timeout_exception.what() << std::endl;
04290       throw;
04291     }
04292     
04293     /* Catch any I/O exceptions */
04294     catch(SickIOException &sick_io_exception) {
04295       std::cerr << sick_io_exception.what() << std::endl;
04296       throw;
04297     }
04298 
04299     /* Catch any thread exceptions */
04300     catch(SickThreadException &sick_thread_exception) {
04301       std::cerr << sick_thread_exception.what() << std::endl;
04302       throw;
04303     }
04304 
04305     /* Catch anything else */
04306     catch(...) {
04307       std::cerr << "SickLMS2xx::_switchSickOperatingMode: Unknown exception!!!" << std::endl;
04308       throw;
04309     }
04310     
04311     /* Reset the buffer */
04312     memset(payload_buffer,0,sizeof(payload_buffer));
04313   
04314     /* Obtain the response payload */
04315     response.GetPayload(payload_buffer);
04316 
04317     /* Make sure the reply was expected */
04318     if(payload_buffer[1] != 0x00) {
04319       throw SickConfigException("SickLMS2xx::_switchSickOperatingMode: configuration request failed!");
04320     }
04321 
04322   }
04323 
04329   void SickLMS2xx::_parseSickScanProfileB0( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_b0_t &sick_scan_profile ) const {
04330 
04331     /* Read block A, the number of measurments */
04332     sick_scan_profile.sick_num_measurements = src_buffer[0] + 256*(src_buffer[1] & 0x03);
04333 
04334     /* Check whether this is a partial scan */
04335     sick_scan_profile.sick_partial_scan_index = ((src_buffer[1] & 0x18) >> 3);
04336 
04337     /* Extract the measurements and Field values (if there are any) */
04338     _extractSickMeasurementValues(&src_buffer[2],
04339                                   sick_scan_profile.sick_num_measurements,
04340                                   sick_scan_profile.sick_measurements,
04341                                   sick_scan_profile.sick_field_a_values,
04342                                   sick_scan_profile.sick_field_b_values,
04343                                   sick_scan_profile.sick_field_c_values);
04344     
04345     /* If the Sick is pulling real-time indices then pull them too */
04346     unsigned int data_offset = 2 + 2*sick_scan_profile.sick_num_measurements;
04347     if (_returningRealTimeIndices()) {
04348       sick_scan_profile.sick_real_time_scan_index = src_buffer[data_offset];
04349       data_offset++;
04350     }
04351 
04352     /* Buffer the Sick telegram index */
04353     sick_scan_profile.sick_telegram_index = src_buffer[data_offset];
04354     
04355   }
04356 
04362   void SickLMS2xx::_parseSickScanProfileB6( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_b6_t &sick_scan_profile ) const {
04363 
04364     /* Read Block A, the sample size used in computing the mean return */
04365     sick_scan_profile.sick_sample_size = src_buffer[0];
04366 
04367     /* Read Block B, the number of measured values sent */
04368     sick_scan_profile.sick_num_measurements = src_buffer[1] + 256*(src_buffer[2] & 0x03);
04369 
04370     /* Read Block C, extract the range measurements and Field values (if there are any) */
04371     _extractSickMeasurementValues(&src_buffer[3],
04372                                   sick_scan_profile.sick_num_measurements,
04373                                   sick_scan_profile.sick_measurements);
04374     
04375     /* Read Block D, if the Sick is pulling real-time indices then pull them too */
04376     unsigned int data_offset = 3 + 2*sick_scan_profile.sick_num_measurements;
04377     if (_returningRealTimeIndices()) {
04378       sick_scan_profile.sick_real_time_scan_index = src_buffer[data_offset];
04379       data_offset++;
04380     }
04381 
04382     /* Read Block E, buffer the Sick telegram index */
04383     sick_scan_profile.sick_telegram_index = src_buffer[data_offset];
04384     
04385   }
04386   
04392   void SickLMS2xx::_parseSickScanProfileB7( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_b7_t &sick_scan_profile ) const {
04393 
04394     /* Read Block A, Sick LMS measured value subrange start index */
04395     sick_scan_profile.sick_subrange_start_index = src_buffer[0] + 256*src_buffer[1];
04396 
04397     /* Read Block B, Sick LMS measured value subrange stop index */
04398     sick_scan_profile.sick_subrange_stop_index = src_buffer[2] + 256*src_buffer[3];
04399     
04400     /* Read block C, the number of measurements */
04401     sick_scan_profile.sick_num_measurements = src_buffer[4] + 256*(src_buffer[5] & 0x03);
04402 
04403     /* Acquire the partial scan index (also in Block C) */
04404     sick_scan_profile.sick_partial_scan_index = ((src_buffer[5] & 0x18) >> 3);
04405     
04406     /* Read Block D, extract the range measurements and Field values (if there are any) */
04407     _extractSickMeasurementValues(&src_buffer[6],
04408                                   sick_scan_profile.sick_num_measurements,
04409                                   sick_scan_profile.sick_measurements,
04410                                   sick_scan_profile.sick_field_a_values,
04411                                   sick_scan_profile.sick_field_b_values,
04412                                   sick_scan_profile.sick_field_c_values);
04413     
04414     /* Read Block E, if the Sick is pulling real-time indices then pull them too */
04415     unsigned int data_offset = 6 + 2*sick_scan_profile.sick_num_measurements;
04416     if (_returningRealTimeIndices()) {
04417       sick_scan_profile.sick_real_time_scan_index = src_buffer[data_offset];
04418       data_offset++;
04419     }
04420 
04421     /* Read Block F, buffer the Sick telegram index */
04422     sick_scan_profile.sick_telegram_index = src_buffer[data_offset];
04423     
04424   }
04425 
04431   void SickLMS2xx::_parseSickScanProfileBF( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_bf_t &sick_scan_profile ) const {
04432 
04433     /* Read Block A, the sample size used in computing the mean return */
04434     sick_scan_profile.sick_sample_size = src_buffer[0];
04435 
04436     /* Read Block B, Sick LMS measured value subrange start index */
04437     sick_scan_profile.sick_subrange_start_index = src_buffer[1] + 256*src_buffer[2];
04438 
04439     /* Read Block C, Sick LMS measured value subrange stop index */
04440     sick_scan_profile.sick_subrange_stop_index = src_buffer[3] + 256*src_buffer[4];
04441     
04442     /* Read Block D, the number of measured values sent */
04443     sick_scan_profile.sick_num_measurements = src_buffer[5] + 256*(src_buffer[6] & 0x3F);
04444 
04445     /* Read Block E, extract the mean measurements */
04446     _extractSickMeasurementValues(&src_buffer[7],
04447                                   sick_scan_profile.sick_num_measurements,
04448                                   sick_scan_profile.sick_measurements);
04449     
04450     /* Read Block D, if the Sick is pulling real-time indices then pull them too */
04451     unsigned int data_offset = 7 + 2*sick_scan_profile.sick_num_measurements;
04452     if (_returningRealTimeIndices()) {
04453       sick_scan_profile.sick_real_time_scan_index = src_buffer[data_offset];
04454       data_offset++;
04455     }
04456 
04457     /* Read Block E, buffer the Sick telegram index */
04458     sick_scan_profile.sick_telegram_index = src_buffer[data_offset];
04459     
04460   }
04461   
04467   void SickLMS2xx::_parseSickScanProfileC4( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_c4_t &sick_scan_profile ) const {
04468 
04469     /* Read block A - the number of range measurments.  We need the low two bits
04470      * of the most significant byte. */
04471     sick_scan_profile.sick_num_range_measurements = src_buffer[0] + 256*(src_buffer[1] & 0x03);
04472 
04473     /* Read Block B - Extract the range measurements and Field values (if there are any) */
04474     _extractSickMeasurementValues(&src_buffer[2],
04475                                   sick_scan_profile.sick_num_range_measurements,
04476                                   sick_scan_profile.sick_range_measurements,
04477                                   sick_scan_profile.sick_field_a_values,
04478                                   sick_scan_profile.sick_field_b_values,
04479                                   sick_scan_profile.sick_field_c_values);
04480     
04481     /* Read Block D - Extract the number of range measurements */
04482     unsigned int data_offset = 2 + 2*sick_scan_profile.sick_num_range_measurements;
04483     sick_scan_profile.sick_num_reflect_measurements = src_buffer[data_offset] + 256*(src_buffer[data_offset+1] & 0x03);
04484     data_offset += 2;
04485     
04486     /* Read Block E - Extract the reflectivity value subrange start index */
04487     sick_scan_profile.sick_reflect_subrange_start_index = src_buffer[data_offset] + 256*src_buffer[data_offset+1];
04488     data_offset += 2;
04489 
04490     /* Read Block F - Extract the reflectivity value subrange stop index */
04491     sick_scan_profile.sick_reflect_subrange_stop_index = src_buffer[data_offset] + 256*src_buffer[data_offset+1];
04492     data_offset += 2;
04493 
04494     /* Read blocks G...H. Mask out the reflectivity values and store in provided buffer. */
04495     for(unsigned int i=0; i < sick_scan_profile.sick_num_reflect_measurements; i++,data_offset++) {
04496       sick_scan_profile.sick_reflect_measurements[i] = src_buffer[data_offset];
04497     }
04498     
04499     /* Read Block I - real-time scan indices */
04500     if (_returningRealTimeIndices()) {
04501       sick_scan_profile.sick_real_time_scan_index = src_buffer[data_offset];
04502       data_offset++;
04503     }
04504 
04505     /* Read Block J - the telegram scan index */
04506     sick_scan_profile.sick_telegram_index = src_buffer[data_offset];
04507     
04508   }
04509   
04515   void SickLMS2xx::_parseSickConfigProfile( const uint8_t * const src_buffer, sick_lms_2xx_device_config_t &sick_device_config ) const {
04516 
04517     /* Buffer Block A */
04518     memcpy(&sick_device_config.sick_blanking,&src_buffer[0],2);
04519     sick_device_config.sick_blanking = sick_lms_2xx_to_host_byte_order(sick_device_config.sick_blanking);
04520     
04521     /* Buffer Block B */
04522     sick_device_config.sick_peak_threshold = src_buffer[3]; // NOTE: This value represent sensitivity for LMS 211/221/291
04523     sick_device_config.sick_stop_threshold = src_buffer[2]; // NOTE: This value will be 0 for LMS 211/221/291
04524     
04525     /* Buffer Block C */
04526     sick_device_config.sick_availability_level = src_buffer[4];
04527     
04528     /* Buffer Block D */
04529     sick_device_config.sick_measuring_mode = src_buffer[5];
04530     
04531     /* Buffer Block E */
04532     sick_device_config.sick_measuring_units = src_buffer[6];
04533     
04534     /* Buffer Block F */
04535     sick_device_config.sick_temporary_field = src_buffer[7];
04536     
04537     /* Buffer Block G */
04538     sick_device_config.sick_subtractive_fields = src_buffer[8];
04539     
04540     /* Buffer Block H */
04541     sick_device_config.sick_multiple_evaluation = src_buffer[9];
04542     
04543     /* Buffer Block I */
04544     sick_device_config.sick_restart = src_buffer[10];
04545     
04546     /* Buffer Block J */
04547     sick_device_config.sick_restart_time = src_buffer[11];
04548     
04549     /* Buffer Block K */
04550     sick_device_config.sick_multiple_evaluation_suppressed_objects = src_buffer[12];
04551     
04552     /* Buffer Block L */
04553     sick_device_config.sick_contour_a_reference = src_buffer[13];
04554     
04555     /* Buffer Block M */
04556     sick_device_config.sick_contour_a_positive_tolerance_band = src_buffer[14];
04557     
04558     /* Buffer Block N */
04559     sick_device_config.sick_contour_a_negative_tolerance_band = src_buffer[15];
04560     
04561     /* Buffer Block O */
04562     sick_device_config.sick_contour_a_start_angle = src_buffer[16];
04563     
04564     /* Buffer Block P */
04565     sick_device_config.sick_contour_a_stop_angle = src_buffer[17];
04566     
04567     /* Buffer Block Q */
04568     sick_device_config.sick_contour_b_reference = src_buffer[18];
04569     
04570     /* Buffer Block R */
04571     sick_device_config.sick_contour_b_positive_tolerance_band = src_buffer[19];
04572     
04573     /* Buffer Block S */
04574     sick_device_config.sick_contour_b_negative_tolerance_band = src_buffer[20];
04575     
04576     /* Buffer Block T */
04577     sick_device_config.sick_contour_b_start_angle = src_buffer[21];
04578     
04579     /* Buffer Block U */
04580     sick_device_config.sick_contour_b_stop_angle = src_buffer[22];
04581     
04582     /* Buffer Block V */
04583     sick_device_config.sick_contour_c_reference = src_buffer[23];
04584     
04585     /* Buffer Block W */
04586     sick_device_config.sick_contour_c_positive_tolerance_band = src_buffer[24];
04587 
04588     /* Buffer Block X */
04589     sick_device_config.sick_contour_c_negative_tolerance_band = src_buffer[25];
04590     
04591     /* Buffer Block Y */
04592     sick_device_config.sick_contour_c_start_angle = src_buffer[26];
04593     
04594     /* Buffer Block Z */
04595     sick_device_config.sick_contour_c_stop_angle = src_buffer[27];
04596     
04597     /* Buffer Block A1 */
04598     sick_device_config.sick_pixel_oriented_evaluation = src_buffer[28];
04599     
04600     /* Buffer Block A2 */
04601     sick_device_config.sick_single_measured_value_evaluation_mode = src_buffer[29];
04602     
04603     /* Buffer Block A3 */
04604     memcpy(&sick_device_config.sick_fields_b_c_restart_times,&src_buffer[30],2);
04605     sick_device_config.sick_fields_b_c_restart_times =
04606       sick_lms_2xx_to_host_byte_order(sick_device_config.sick_fields_b_c_restart_times);
04607     
04608     /* Buffer Block A4 */
04609     memcpy(&sick_device_config.sick_dazzling_multiple_evaluation,&src_buffer[32],2);
04610     sick_device_config.sick_dazzling_multiple_evaluation =
04611       sick_lms_2xx_to_host_byte_order(sick_device_config.sick_dazzling_multiple_evaluation);
04612     
04613   }
04614 
04624   void SickLMS2xx::_extractSickMeasurementValues( const uint8_t * const byte_sequence, const uint16_t num_measurements, uint16_t * const measured_values,
04625                                                uint8_t * const field_a_values, uint8_t * const field_b_values, uint8_t * const field_c_values ) const {
04626 
04627     /* Parse the byte sequence and fill the return buffer with range measurements... */   
04628     switch(_sick_device_config.sick_measuring_mode) {
04629     case SICK_MS_MODE_8_OR_80_FA_FB_DAZZLE:
04630       {
04631 
04632         /* Extract the range and Field values */
04633         for(unsigned int i = 0; i < num_measurements; i++) {
04634           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x1F);
04635 
04636           if(field_a_values) {  
04637             field_a_values[i] = byte_sequence[i*2+1] & 0x20;
04638           }
04639           
04640           if(field_b_values) {
04641             field_b_values[i] = byte_sequence[i*2+1] & 0x40;
04642           }
04643           
04644           if(field_c_values) {
04645             field_c_values[i] = byte_sequence[i*2+1] & 0x80;
04646           }
04647           
04648         }
04649         
04650         break;
04651       }
04652     case SICK_MS_MODE_8_OR_80_REFLECTOR:
04653       {
04654         
04655         /* Extract the range and Field A */
04656         for(unsigned int i = 0; i < num_measurements; i++) {
04657           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x1F);
04658           
04659           if(field_a_values) {
04660             field_a_values[i] = byte_sequence[i*2+1] & 0xE0;
04661           }
04662           
04663         }
04664         
04665         break;
04666       }     
04667     case SICK_MS_MODE_8_OR_80_FA_FB_FC:
04668       {
04669         
04670         /* Extract the range and Fields A,B and C */    
04671         for(unsigned int i = 0; i < num_measurements; i++) {
04672           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x1F);
04673           
04674           if(field_a_values) {
04675             field_a_values[i] = byte_sequence[i*2+1] & 0x20;
04676           }
04677           
04678           if(field_b_values) {
04679             field_b_values[i] = byte_sequence[i*2+1] & 0x40;
04680           }
04681           
04682           if(field_c_values) {
04683             field_c_values[i] = byte_sequence[i*2+1] & 0x80;
04684           }
04685           
04686         }
04687         
04688         break;
04689       }
04690     case SICK_MS_MODE_16_REFLECTOR:
04691       {
04692 
04693         /* Extract the range and reflector values */
04694         for(unsigned int i = 0; i < num_measurements; i++) {
04695           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x3F);
04696 
04697           if (field_a_values) {
04698             field_a_values[i] = byte_sequence[i*2+1] & 0xC0;
04699           }
04700           
04701         }
04702         
04703         break;
04704       }
04705     case SICK_MS_MODE_16_FA_FB:
04706       {
04707 
04708         /* Extract the range and Fields A and B values */
04709         for(unsigned int i = 0; i < num_measurements; i++) {
04710           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x3F);
04711 
04712           if(field_a_values) {
04713             field_a_values[i] = byte_sequence[i*2+1] & 0x40;
04714           }
04715 
04716           if(field_b_values) {
04717             field_b_values[i] = byte_sequence[i*2+1] & 0x80;
04718           }
04719 
04720         }
04721         
04722         break;
04723       }
04724     case SICK_MS_MODE_32_REFLECTOR:
04725       {
04726 
04727         /* Extract the range and reflector values */
04728         for(unsigned int i = 0; i < num_measurements; i++) {
04729           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x7F);
04730 
04731           if(field_a_values) {
04732             field_a_values[i] = byte_sequence[i*2+1] & 0x80;
04733           }
04734           
04735         }
04736         
04737         break;
04738       }
04739     case SICK_MS_MODE_32_FA:
04740       {
04741 
04742         /* Extract the range and Field A values */
04743         for(unsigned int i = 0; i < num_measurements; i++) {
04744           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x7F);
04745 
04746           if(field_a_values) {
04747             field_a_values[i] = byte_sequence[i*2+1] & 0x80;
04748           }
04749           
04750         }
04751         
04752         break;
04753       }
04754     case SICK_MS_MODE_32_IMMEDIATE:
04755       {
04756         
04757         /* Extract the range measurements (no flags for this mode */
04758         for(unsigned int i = 0; i < num_measurements; i++) {
04759           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1]);
04760         }
04761         
04762         break;
04763       }
04764     case SICK_MS_MODE_REFLECTIVITY:
04765       {
04766 
04767         /* Extract the reflectivity values */
04768         for(unsigned int i = 0; i < num_measurements; i++) {
04769           measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1]);
04770         }
04771         
04772         break;
04773       }      
04774     default:      
04775       break;
04776     }
04777     
04778   }
04779   
04784   bool SickLMS2xx::_validSickMeasuringUnits( const sick_lms_2xx_measuring_units_t sick_units ) const {
04785 
04786     /* Check the given units value */
04787     if (sick_units != SICK_MEASURING_UNITS_CM && sick_units != SICK_MEASURING_UNITS_MM) {
04788       return false;
04789     }
04790 
04791     /* Valid */
04792     return true;
04793   }
04794 
04799   bool SickLMS2xx::_isSickLMS200( ) const {
04800 
04801     /* Check the given Sick type value */
04802     switch(_sick_type) {
04803     case SICK_LMS_TYPE_200_30106:
04804       return true;
04805     default:
04806       return false;
04807     }
04808 
04809   }
04810 
04815   bool SickLMS2xx::_isSickLMS211( ) const {
04816 
04817     /* Check the given Sick type value */
04818     switch(_sick_type) {
04819     case SICK_LMS_TYPE_211_30106:
04820       return true;
04821     case SICK_LMS_TYPE_211_30206:
04822       return true;
04823     case SICK_LMS_TYPE_211_S07:
04824       return true;
04825     case SICK_LMS_TYPE_211_S14:
04826       return true;
04827     case SICK_LMS_TYPE_211_S15:
04828       return true;
04829     case SICK_LMS_TYPE_211_S19:
04830       return true;
04831     case SICK_LMS_TYPE_211_S20:
04832       return true;
04833     default:
04834       return false;
04835     }
04836 
04837   }
04838 
04843   bool SickLMS2xx::_isSickLMS220( ) const {
04844 
04845     /* Check the given Sick type value */
04846     switch(_sick_type) {
04847     case SICK_LMS_TYPE_220_30106:
04848       return true;
04849     default:
04850       return false;
04851     }
04852 
04853   }
04854 
04859   bool SickLMS2xx::_isSickLMS221( ) const {
04860 
04861     /* Check the given Sick type value */
04862     switch(_sick_type) {
04863     case SICK_LMS_TYPE_221_30106:
04864       return true;
04865     case SICK_LMS_TYPE_221_30206:
04866       return true;
04867     case SICK_LMS_TYPE_221_S07:
04868       return true;
04869     case SICK_LMS_TYPE_221_S14:
04870       return true;
04871     case SICK_LMS_TYPE_221_S15:
04872       return true;
04873     case SICK_LMS_TYPE_221_S16:
04874       return true;
04875     case SICK_LMS_TYPE_221_S19:
04876       return true;
04877     case SICK_LMS_TYPE_221_S20:
04878       return true;
04879     default:
04880       return false;
04881     }
04882 
04883   }
04884 
04889   bool SickLMS2xx::_isSickLMS291( ) const {
04890 
04891     /* Check the given Sick type value */
04892     switch(_sick_type) {
04893     case SICK_LMS_TYPE_291_S05:
04894       return true;
04895     case SICK_LMS_TYPE_291_S14:
04896       return true;
04897     case SICK_LMS_TYPE_291_S15:
04898       return true;
04899     default:
04900       return false;
04901     }
04902 
04903   }
04904 
04909   bool SickLMS2xx::_isSickUnknown( ) const {
04910     return _sick_type == SICK_LMS_TYPE_UNKNOWN;
04911   }
04912     
04917   bool SickLMS2xx::_validSickScanAngle( const sick_lms_2xx_scan_angle_t sick_scan_angle ) const {
04918 
04919     /* Check the given Sick scan angle */
04920     if (sick_scan_angle != SICK_SCAN_ANGLE_90 &&
04921         sick_scan_angle != SICK_SCAN_ANGLE_100 &&
04922         sick_scan_angle != SICK_SCAN_ANGLE_180 ) {
04923       
04924       return false;
04925     }
04926 
04927     /* Valid */
04928     return true;
04929   }
04930 
04935   bool SickLMS2xx::_validSickScanResolution( const sick_lms_2xx_scan_resolution_t sick_scan_resolution ) const {
04936 
04937     /* Check the given Sick scan resolution value */
04938     if (sick_scan_resolution != SICK_SCAN_RESOLUTION_25 &&
04939         sick_scan_resolution != SICK_SCAN_RESOLUTION_50 &&
04940         sick_scan_resolution != SICK_SCAN_RESOLUTION_100 ) {
04941       
04942       return false;
04943     }
04944 
04945     /* Valid */
04946     return true;
04947   }
04948   
04953   bool SickLMS2xx::_validSickSensitivity( const sick_lms_2xx_sensitivity_t sick_sensitivity ) const {
04954 
04955     /* Check the given Sick sensitivity value */
04956     if (sick_sensitivity != SICK_SENSITIVITY_STANDARD &&
04957         sick_sensitivity != SICK_SENSITIVITY_MEDIUM &&
04958         sick_sensitivity != SICK_SENSITIVITY_LOW &&
04959         sick_sensitivity != SICK_SENSITIVITY_HIGH ) {
04960       
04961       return false;
04962     }
04963 
04964     /* Valid */
04965     return true;
04966   }
04967 
04972   bool SickLMS2xx::_validSickPeakThreshold( const sick_lms_2xx_peak_threshold_t sick_peak_threshold ) const {
04973 
04974     /* Check the given Sick scan angle */
04975     if (sick_peak_threshold != SICK_PEAK_THRESHOLD_DETECTION_WITH_NO_BLACK_EXTENSION &&
04976         sick_peak_threshold != SICK_PEAK_THRESHOLD_DETECTION_WITH_BLACK_EXTENSION &&
04977         sick_peak_threshold != SICK_PEAK_THRESHOLD_NO_DETECTION_WITH_NO_BLACK_EXTENSION &&
04978         sick_peak_threshold != SICK_PEAK_THRESHOLD_NO_DETECTION_WITH_BLACK_EXTENSION) {
04979       
04980       return false;
04981     }
04982 
04983     /* Valid */
04984     return true;
04985   }
04986   
04991   bool SickLMS2xx::_validSickMeasuringMode( const sick_lms_2xx_measuring_mode_t sick_measuring_mode ) const {
04992 
04993     /* Check the given measuring mode */
04994     if (sick_measuring_mode != SICK_MS_MODE_8_OR_80_FA_FB_DAZZLE &&
04995         sick_measuring_mode != SICK_MS_MODE_8_OR_80_REFLECTOR &&
04996         sick_measuring_mode != SICK_MS_MODE_8_OR_80_FA_FB_FC &&
04997         sick_measuring_mode != SICK_MS_MODE_16_REFLECTOR &&
04998         sick_measuring_mode != SICK_MS_MODE_16_FA_FB &&
04999         sick_measuring_mode != SICK_MS_MODE_32_REFLECTOR &&
05000         sick_measuring_mode != SICK_MS_MODE_32_FA &&
05001         sick_measuring_mode != SICK_MS_MODE_32_IMMEDIATE &&
05002         sick_measuring_mode != SICK_MS_MODE_REFLECTIVITY ) {
05003 
05004       return false;
05005     }
05006     
05007     /* Valid */
05008     return true;
05009   }
05010   
05016   sick_lms_2xx_baud_t SickLMS2xx::_baudToSickBaud( const int baud_rate ) const {
05017   
05018     switch(baud_rate) {
05019     case B9600:
05020       return SICK_BAUD_9600;
05021     case B19200:
05022       return SICK_BAUD_19200;
05023     case B38400:
05024       return SICK_BAUD_38400;
05025     case B500000:
05026       return SICK_BAUD_500K;
05027     default:
05028       std::cerr << "Unexpected baud rate!" << std::endl;
05029       return SICK_BAUD_9600;
05030     }
05031     
05032   }
05033 
05039   std::string SickLMS2xx::_sickAvailabilityToString( const uint8_t availability_flags ) const {
05040 
05041     /* Check if availability is specified */
05042     if (availability_flags == 0) {
05043       return "Default (Unspecified)";
05044     }
05045 
05046     std::string availability_str;
05047     
05048     /* Check if set to highest possible availability */
05049     if (0x01 & availability_flags) {
05050       availability_str += "Highest";
05051     }
05052 
05053     /* Check for real-time indices */
05054     if (0x02 & availability_flags) {
05055 
05056       /* Check whether to add a spacer */
05057       if (availability_str.length() > 0) {
05058         availability_str += ", ";
05059       }      
05060 
05061       availability_str += "Real-time indices";
05062     }
05063 
05064     /* Check for real-time indices */
05065     if (0x04 & availability_flags) {
05066 
05067       /* Check whether to add a spacer */
05068       if (availability_str.length() > 0) {
05069         availability_str += ", ";
05070       }
05071 
05072       availability_str += "No effect dazzle";
05073     }
05074 
05075     /* Return the string */
05076     return availability_str;
05077     
05078   }
05079     
05085   std::string SickLMS2xx::_sickRestartToString( const uint8_t restart_code ) const {
05086 
05087     std::string restart_str;
05088 
05089     /* Identify restart mode */
05090     switch(restart_code) {
05091     case 0x00:
05092       restart_str += "Restart when button actuated";
05093       break;
05094     case 0x01:
05095       restart_str += "Restart after set time";
05096       break;
05097     case 0x02:
05098       restart_str += "No restart block";
05099       break;
05100     case 0x03:
05101       restart_str += "Button switches field set, restart after set time";
05102       break;
05103     case 0x04:
05104       restart_str += "Button switches field set, no restart block";
05105       break;
05106     case 0x05:
05107       restart_str += "LMS2xx operates as a slave, restart after set time";
05108       break;
05109     case 0x06:
05110       restart_str += "LMS2xx operates as a slave, immediate restart";
05111       break;
05112     default:
05113       restart_str += "Unknown!";      
05114     }
05115 
05116     /* Return the restart code */
05117     return restart_str;
05118     
05119   }
05120 
05126   std::string SickLMS2xx::_sickTemporaryFieldToString( const uint8_t temp_field_code ) const {
05127 
05128     switch(temp_field_code) {
05129     case 0:
05130       return "Not used";
05131     case 1:
05132       return "Belongs to field set no. 1";
05133     case 2:
05134       return "Belongs to field set no. 2";
05135     default:
05136       return "Unknown!";
05137     }
05138 
05139   }
05140 
05146   std::string SickLMS2xx::_sickSubtractiveFieldsToString( const uint8_t subt_field_code ) const {
05147 
05148     switch(subt_field_code) {
05149     case 0:
05150       return "Not active";
05151     case 1:
05152       return "Active";
05153     default:
05154       return "Unknown!";
05155     }
05156 
05157   }
05158 
05164   std::string SickLMS2xx::_sickContourFunctionToString( const uint8_t contour_function_code ) const {
05165 
05166     switch(contour_function_code) {
05167     case 0:
05168       return "Not active";
05169     default: {
05170 
05171       /* For converting an int to string */
05172       std::ostringstream output_str;
05173 
05174       /* Indicate its active and include min object size */
05175       output_str << "Active, Min object size: " << (int)contour_function_code << " (cm)";
05176       return output_str.str();
05177       
05178     }
05179     }
05180     
05181   }
05182   
05188   std::string SickLMS2xx::_sickVariantToString( const unsigned int sick_variant ) const {
05189 
05190     /* Return the correct string */
05191     if(sick_variant == SICK_LMS_VARIANT_2XX_TYPE_6) {
05192       return "Standard device (LMS2xx,type 6)";
05193     }
05194     else if (sick_variant == SICK_LMS_VARIANT_SPECIAL) {
05195       return "Special device (LMS211-/221-S19/-S20)";
05196     }
05197     else {
05198       return "Unknown";
05199     }
05200     
05201   }
05202 
05203 } //namespace SickToolbox


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