SickLD.cc
Go to the documentation of this file.
00001 
00016 /* Auto-generated header */
00017 #include <sicktoolbox/SickConfig.hh>
00018 
00019 /* Implementation dependencies */
00020 #include <string>
00021 #include <cmath>
00022 #include <iostream>
00023 #include <iomanip>
00024 #include <sstream>            // for converting numerical values to strings
00025 #include <sys/socket.h>       // for socket function definitions
00026 #include <arpa/inet.h>        // for sockaddr_in, inet_addr, and htons
00027 #include <sys/ioctl.h>        // for using ioctl functionality for the socket input buffer
00028 #include <unistd.h>           // for select functionality (e.g. FD_SET, etc...)
00029 #include <sys/types.h>        // for fd data types
00030 #include <sys/time.h>         // for select timeout parameter
00031 #include <fcntl.h>            // for getting file flags
00032 #include <pthread.h>          // for POSIX threads
00033 #include <sstream>            // for parsing ip addresses
00034 #include <vector>             // for returning the results of parsed strings
00035 #include <errno.h>            // for timing connect()
00036 
00037 #include <sicktoolbox/SickLD.hh>
00038 #include <sicktoolbox/SickLDMessage.hh>
00039 #include <sicktoolbox/SickLDBufferMonitor.hh>
00040 #include <sicktoolbox/SickLDUtility.hh>   
00041 #include <sicktoolbox/SickException.hh>
00042 
00043 /* Associate the namespace */
00044 namespace SickToolbox {
00045 
00051   SickLD::SickLD( const std::string sick_ip_address, const uint16_t sick_tcp_port ) :
00052     SickLIDAR< SickLDBufferMonitor, SickLDMessage >( ),
00053     _sick_ip_address(sick_ip_address),
00054     _sick_tcp_port(sick_tcp_port),
00055     _sick_sensor_mode(SICK_SENSOR_MODE_UNKNOWN),
00056     _sick_motor_mode(SICK_MOTOR_MODE_UNKNOWN),
00057     _sick_streaming_range_data(false),
00058     _sick_streaming_range_and_echo_data(false)
00059   {
00060     /* Initialize the sick identity */
00061     _sick_identity.sick_part_number =
00062       _sick_identity.sick_name =
00063       _sick_identity.sick_version = 
00064       _sick_identity.sick_serial_number =
00065       _sick_identity.sick_edm_serial_number =
00066       _sick_identity.sick_firmware_part_number =
00067       _sick_identity.sick_firmware_name =
00068       _sick_identity.sick_firmware_version =
00069       _sick_identity.sick_application_software_part_number =
00070       _sick_identity.sick_application_software_name =
00071       _sick_identity.sick_application_software_version = "UNKNOWN";
00072 
00073     /* Initialize the global configuration structure */
00074     memset(&_sick_global_config,0,sizeof(sick_ld_config_global_t));
00075 
00076     /* Initialize the Ethernet configuration structure */
00077     memset(&_sick_ethernet_config,0,sizeof(sick_ld_config_ethernet_t));
00078 
00079     /* Initialize the sector configuration structure */
00080     memset(&_sick_sector_config,0,sizeof(sick_ld_config_sector_t));
00081   }
00082 
00086   SickLD::~SickLD( ) { }
00087 
00091   void SickLD::Initialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException ) {
00092 
00093     std::cout << "\t*** Attempting to initialize the Sick LD..." << std::endl; 
00094 
00095     try {
00096       
00097       /* Attempt to connect to the Sick LD */
00098       std::cout << "\tAttempting to connect to Sick LD @ " << _sick_ip_address << ":" << _sick_tcp_port << std::endl;
00099       _setupConnection();
00100       std::cout << "\t\tConnected to Sick LD!" << std::endl;
00101 
00102       /* Start the buffer monitor */
00103       std::cout << "\tAttempting to start buffer monitor..." << std::endl;
00104       _startListening();
00105       std::cout << "\t\tBuffer monitor started!" << std::endl;
00106     
00107       /* Ok, lets sync the driver with the Sick */
00108       std::cout << "\tAttempting to sync driver with Sick LD..." << std::endl;
00109       _syncDriverWithSick();
00110       
00111     }
00112     
00113     catch(SickIOException &sick_io_exception) {
00114       std::cerr << sick_io_exception.what() << std::endl;
00115       throw;
00116     }
00117     
00118     catch(SickThreadException &sick_thread_exception) {
00119       std::cerr << sick_thread_exception.what() << std::endl;
00120       throw;
00121     }
00122 
00123     catch(SickTimeoutException &sick_timeout_exception) {
00124       std::cerr << sick_timeout_exception.what() << std::endl;
00125       throw;
00126     }
00127 
00128     catch(...) {
00129       std::cerr << "SickLD::Initialize - Unknown exception!" << std::endl;
00130       throw;
00131     }
00132     
00133     std::cout << "\t\tSynchronized!" << std::endl;
00134 
00135     _sick_initialized = true;
00136     _printInitFooter();
00137 
00138     /* Success */
00139   }
00140 
00146   void SickLD::GetSickStatus( unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode )
00147     throw( SickIOException, SickTimeoutException ){
00148 
00149     /* Ensure the device has been initialized */
00150     if (!_sick_initialized) {
00151       throw SickIOException("SickLD::GetSickStatus: Device NOT Initialized!!!");
00152     }
00153   
00154     /* Acquire the sensor and motor mode from the device */
00155     try {
00156       _getSickStatus();
00157     }
00158 
00159     catch(SickTimeoutException &sick_timeout_exception) {
00160       std::cerr << sick_timeout_exception.what() << std::endl;
00161       throw;
00162     }
00163 
00164     catch(SickIOException &sick_io_exception) {
00165       std::cerr << sick_io_exception.what() << std::endl;
00166       throw;
00167     }
00168 
00169     catch(...) {
00170       std::cerr << "SickLD::GetSickStatus - Unknown exception!" << std::endl;
00171       throw;
00172     }
00173     
00174     /* Now that the driver is updated, assign the return values */
00175     sick_sensor_mode = _sick_sensor_mode;
00176     sick_motor_mode = _sick_motor_mode;
00177   
00178     /* Success */
00179   }
00180 
00190   void SickLD::SetSickTempScanAreas( const double * active_sector_start_angles, const double * active_sector_stop_angles,
00191                                      const unsigned int num_active_sectors )
00192         throw( SickTimeoutException, SickIOException, SickConfigException ) {
00193 
00194     /* Ensure the device has been initialized */
00195     if (!_sick_initialized) {
00196       throw SickConfigException("SickLD::SetSickTempScanAreas: Device NOT Initialized!!!");
00197     }
00198     
00199     /* Do the standard initialization */
00200     try {
00201       
00202       /* Set the temporary scan configuration */
00203       std::cout << "\tAttempting to set desired scan config..." << std::endl;
00204       _setSickTemporaryScanAreas(active_sector_start_angles,active_sector_stop_angles,num_active_sectors);
00205       std::cout << "\t\tUsing desired scan area(s)!" << std::endl;
00206 
00207     }
00208     
00209     catch(SickIOException &sick_io_exception) {
00210       std::cerr << sick_io_exception.what() << std::endl;
00211       throw;
00212     }
00213     
00214     catch(SickTimeoutException &sick_timeout_exception) {
00215       std::cerr << sick_timeout_exception.what() << std::endl;
00216       throw;
00217     }
00218 
00219     catch(SickConfigException &sick_config_exception) {
00220       std::cerr << sick_config_exception.what() << std::endl;
00221       throw;
00222     }
00223     
00224     catch(...) {
00225       std::cerr << "SickLD::Initialize - Unknown exception!" << std::endl;
00226       throw;
00227     }
00228     
00229     /* Success */    
00230   }
00231   
00232   
00241   void SickLD::SetSickTimeAbsolute( const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time ) 
00242     throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) {
00243 
00244     /* Ensure the device has been initialized */
00245     if (!_sick_initialized) {
00246       throw SickConfigException("SickLD::SetSickTimeAbsolute: Device NOT Initialized!!!");
00247     }
00248   
00249     /* Ensure the device is not in MEASURE mode */
00250     if (_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) {
00251 
00252       /* If it is then set it to rotate */
00253       try {
00254         _setSickSensorModeToRotate();
00255       }
00256                
00257       /* Handle a timeout! */
00258       catch (SickTimeoutException &sick_timeout_exception) {
00259         std::cerr << sick_timeout_exception.what() << std::endl;
00260         throw;
00261       }
00262       
00263       /* Handle I/O exceptions */
00264       catch (SickIOException &sick_io_exception) {
00265         std::cerr << sick_io_exception.what() << std::endl;
00266         throw;
00267       }
00268       
00269       /* Handle a returned error code */
00270       catch (SickErrorException &sick_error_exception) {
00271         std::cerr << sick_error_exception.what() << std::endl;
00272         throw;
00273       }
00274       
00275       /* A safety net */
00276       catch (...) {
00277         std::cerr << "SickLMS::SetSickTimeAbsolute: Unknown exception!!!" << std::endl;
00278         throw;
00279       }  
00280       
00281     }
00282   
00283     std::cout << "\tSetting Sick LD absolute clock time..." << std::endl;
00284   
00285     /* Allocate a single buffer for payload contents */
00286     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00287 
00288     /* Set the service codes */
00289     payload_buffer[0] = SICK_CONF_SERV_CODE;
00290     payload_buffer[1] = SICK_CONF_SERV_SET_TIME_ABSOLUTE;
00291 
00292     /* Set the new time value */
00293     uint16_t temp_buffer = host_to_sick_ld_byte_order(absolute_clock_time);
00294     memcpy(&payload_buffer[2],&temp_buffer,2);
00295 
00296     /* Create the Sick LD send/receive message objects */
00297     SickLDMessage send_message(payload_buffer,4);
00298     SickLDMessage recv_message;  
00299   
00300     /* Send the message and check for the reply */
00301     try {
00302       _sendMessageAndGetReply(send_message,recv_message);
00303     }
00304             
00305     /* Handle a timeout! */
00306     catch (SickTimeoutException &sick_timeout_exception) {
00307       std::cerr << sick_timeout_exception.what() << std::endl;
00308       throw;
00309     }
00310     
00311     /* Handle I/O exceptions */
00312     catch (SickIOException &sick_io_exception) {
00313       std::cerr << sick_io_exception.what() << std::endl;
00314       throw;
00315     }
00316     
00317     /* A safety net */
00318     catch (...) {
00319       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
00320       throw;
00321     }
00322 
00323     /* Reset the payload buffer */
00324     memset(payload_buffer,0,4);
00325 
00326     /* Get the message payload */
00327     recv_message.GetPayload(payload_buffer);
00328   
00329     /* Extract the new Sick LD clock time from the response */
00330     uint16_t clock_time;
00331     memcpy(&clock_time,&payload_buffer[2],2);
00332     new_sick_clock_time = sick_ld_to_host_byte_order(clock_time);
00333 
00334     std::cout << "\t\tClock time set!" << std::endl;
00335   
00336     /* Success */
00337   }
00338 
00347   void SickLD::SetSickTimeRelative( const int16_t delta_time, uint16_t &new_sick_clock_time ) 
00348     throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) {
00349 
00350     /* Ensure the device has been initialized */
00351     if (!_sick_initialized) {
00352       throw SickConfigException("SickLD::SetSickTimeRelative: Device NOT Initialized!!!");
00353     }
00354   
00355     /* Ensure the device is not in MEASURE mode */
00356     if (_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) {
00357       
00358       /* If it is then set it to rotate */
00359       try {
00360         _setSickSensorModeToRotate();
00361       }
00362       
00363       /* Handle a timeout! */
00364       catch (SickTimeoutException &sick_timeout_exception) {
00365         std::cerr << sick_timeout_exception.what() << std::endl;
00366         throw;
00367       }
00368       
00369       /* Handle I/O exceptions */
00370       catch (SickIOException &sick_io_exception) {
00371         std::cerr << sick_io_exception.what() << std::endl;
00372         throw;
00373       }
00374       
00375       /* Handle a returned error code */
00376       catch (SickErrorException &sick_error_exception) {
00377         std::cerr << sick_error_exception.what() << std::endl;
00378         throw;
00379       }
00380       
00381       /* A safety net */
00382       catch (...) {
00383         std::cerr << "SickLMS::SetSickTimeRelative: Unknown exception!!!" << std::endl;
00384         throw;
00385       }  
00386       
00387     }
00388     
00389     std::cout << "\tSetting Sick LD relative clock time..." << std::endl;
00390   
00391     /* Allocate a single buffer for payload contents */
00392     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00393 
00394     /* Set the service codes */
00395     payload_buffer[0] = SICK_CONF_SERV_CODE;
00396     payload_buffer[1] = SICK_CONF_SERV_SET_TIME_RELATIVE;
00397 
00398     /* Set the new time value */
00399     uint16_t temp_buffer = host_to_sick_ld_byte_order((uint16_t)delta_time);
00400     memcpy(&payload_buffer[2],&temp_buffer,2);
00401 
00402     /* Create the Sick LD send/receive message objects */
00403     SickLDMessage send_message(payload_buffer,4);
00404     SickLDMessage recv_message;
00405   
00406     /* Send the message and check for the expected reply */
00407     try {
00408       _sendMessageAndGetReply(send_message,recv_message);
00409     }
00410             
00411     /* Handle a timeout! */
00412     catch (SickTimeoutException &sick_timeout_exception) {
00413       std::cerr << sick_timeout_exception.what() << std::endl;
00414       throw;
00415     }
00416     
00417     /* Handle I/O exceptions */
00418     catch (SickIOException &sick_io_exception) {
00419       std::cerr << sick_io_exception.what() << std::endl;
00420       throw;
00421     }
00422     
00423     /* A safety net */
00424     catch (...) {
00425       std::cerr << "SickLMS::_setSickTimeRelative: Unknown exception!!!" << std::endl;
00426       throw;
00427     }
00428 
00429     /* Reset the payload buffer */
00430     memset(payload_buffer,0,4);
00431 
00432     /* Get the message payload */
00433     recv_message.GetPayload(payload_buffer);
00434   
00435     /* Extract the new Sick LD clock time from the response */
00436     uint16_t clock_time;
00437     memcpy(&clock_time,&payload_buffer[2],2);
00438     new_sick_clock_time = sick_ld_to_host_byte_order(clock_time);
00439 
00440     std::cout << "\t\tClock time set!" << std::endl;
00441   
00442     /* Success */
00443   }
00444 
00453   void SickLD::SetSickSignals( const uint8_t sick_signal_flags )
00454     throw( SickIOException, SickTimeoutException, SickErrorException ) {
00455 
00456     /* Ensure the device has been initialized */
00457     if (!_sick_initialized) {
00458       throw SickConfigException("SickLD::SetSickSignals: Device NOT Initialized!!!");
00459     }
00460 
00461     /* Attempt to set the signal flags */
00462     try {
00463       _setSickSignals(sick_signal_flags);
00464     }
00465 
00466     /* Handle a timeout! */
00467     catch (SickTimeoutException &sick_timeout_exception) {
00468       std::cerr << sick_timeout_exception.what() << std::endl;
00469       throw;
00470     }
00471     
00472     /* Handle I/O exceptions */
00473     catch (SickIOException &sick_io_exception) {
00474       std::cerr << sick_io_exception.what() << std::endl;
00475       throw;
00476     }
00477     
00478     /* Handle a returned error code */
00479     catch (SickErrorException &sick_error_exception) {
00480       std::cerr << sick_error_exception.what() << std::endl;
00481       throw;
00482     }
00483     
00484     /* A safety net */
00485     catch (...) {
00486       std::cerr << "SickLMS::_syncDriverWithSick: Unknown exception!!!" << std::endl;
00487       throw;
00488     } 
00489 
00490     /* Success! */
00491   }
00492 
00497   void SickLD::GetSickSignals( uint8_t &sick_signal_flags ) throw( SickIOException, SickTimeoutException ) {
00498 
00499     /* Ensure the device has been initialized */
00500     if (!_sick_initialized) {
00501       throw SickIOException("SickLD::GetSickSignals: Device NOT Initialized!!!");
00502     }
00503   
00504     /* Initialize the destination buffer */
00505     sick_signal_flags = 0;
00506   
00507     /* Allocate a single buffer for payload contents */
00508     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00509 
00510     /* Set the service IDs */
00511     payload_buffer[0] = SICK_STAT_SERV_CODE;       // Requested service type
00512     payload_buffer[1] = SICK_STAT_SERV_GET_SIGNAL; // Requested service subtype
00513   
00514     /* Create the Sick message */
00515     SickLDMessage send_message(payload_buffer,2);
00516     SickLDMessage recv_message;
00517   
00518     /* Send the message and get the reply */
00519     try {
00520       _sendMessageAndGetReply(send_message,recv_message);
00521     }
00522        
00523     /* Handle a timeout! */
00524     catch (SickTimeoutException &sick_timeout_exception) {
00525       std::cerr << sick_timeout_exception.what() << std::endl;
00526       throw;
00527     }
00528     
00529     /* Handle I/O exceptions */
00530     catch (SickIOException &sick_io_exception) {
00531       std::cerr << sick_io_exception.what() << std::endl;
00532       throw;
00533     }
00534     
00535     /* A safety net */
00536     catch (...) {
00537       std::cerr << "SickLMS::GetSickSignals: Unknown exception!!!" << std::endl;
00538       throw;
00539     }
00540     
00541     /* Reset the payload buffer */
00542     memset(payload_buffer,0,2);
00543   
00544     /* Extract the message payload */
00545     recv_message.GetPayload(payload_buffer);
00546 
00547     /* Extract the Signal flags */
00548     sick_signal_flags = payload_buffer[3];
00549   
00550     /* Success */
00551   }
00552 
00557   void SickLD::GetSickTime( uint16_t &sick_time )
00558     throw( SickIOException, SickTimeoutException, SickErrorException ) {
00559 
00560     /* Ensure the device has been initialized */
00561     if (!_sick_initialized) {
00562       throw SickIOException("SickLD::GetSickTime: Device NOT Initialized!!!");
00563     }
00564   
00565     /* Allocate a single buffer for payload contents */
00566     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00567 
00568     /* Set the service IDs */
00569     payload_buffer[0] = SICK_CONF_SERV_CODE;             // Requested service type
00570     payload_buffer[1] = SICK_CONF_SERV_GET_SYNC_CLOCK;   // Requested service subtype
00571   
00572     /* Create the Sick messages */
00573     SickLDMessage send_message(payload_buffer,2);
00574     SickLDMessage recv_message;
00575 
00576     /* Send the message and check the reply */
00577     try {
00578       _sendMessageAndGetReply(send_message,recv_message);
00579     }
00580        
00581     /* Handle a timeout! */
00582     catch (SickTimeoutException &sick_timeout_exception) {
00583       std::cerr << sick_timeout_exception.what() << std::endl;
00584       throw;
00585     }
00586     
00587     /* Handle I/O exceptions */
00588     catch (SickIOException &sick_io_exception) {
00589       std::cerr << sick_io_exception.what() << std::endl;
00590       throw;
00591     }
00592     
00593     /* A safety net */
00594     catch (...) {
00595       std::cerr << "SickLMS::GetSickTime: Unknown exception!!!" << std::endl;
00596       throw;
00597     }
00598 
00599     /* Reset the payload buffer */
00600     memset(payload_buffer,0,2);
00601   
00602     /* Acquire the returned payload */
00603     recv_message.GetPayload(payload_buffer);
00604 
00605     /* Extract actual time */
00606     uint16_t current_time;
00607     memcpy(&current_time,&payload_buffer[2],2);
00608     sick_time = sick_ld_to_host_byte_order(current_time);
00609 
00610     /* Success */
00611   }
00612 
00619   void SickLD::EnableNearfieldSuppression( )
00620     throw( SickErrorException, SickTimeoutException, SickIOException ) {
00621 
00622     /* Ensure the device has been initialized */
00623     if(!_sick_initialized) {
00624       throw SickIOException("SickLD::EnableNearfieldSuppression: Device NOT Initialized!!!");
00625     }
00626   
00627     /* Tell the Sick LD to use nearfield suppression! */
00628     std::cout << "\tEnabling nearfield suppression..." << std::endl;
00629     try {
00630       _setSickFilter(SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON);
00631     }
00632 
00633     /* Handle a timeout! */
00634     catch (SickTimeoutException &sick_timeout_exception) {
00635       std::cerr << sick_timeout_exception.what() << std::endl;
00636       throw;
00637     }
00638     
00639     /* Handle I/O exceptions */
00640     catch (SickIOException &sick_io_exception) {
00641       std::cerr << sick_io_exception.what() << std::endl;
00642       throw;
00643     }
00644     
00645     /* Handle a returned error code */
00646     catch (SickErrorException &sick_error_exception) {
00647       std::cerr << sick_error_exception.what() << std::endl;
00648       throw;
00649     }
00650     
00651     /* A safety net */
00652     catch (...) {
00653       std::cerr << "SickLMS::EnableNearfieldSuppression: Unknown exception!!!" << std::endl;
00654       throw;
00655     }  
00656     
00657     std::cout << "\t\tSuppression is enabled!" << std::endl;
00658   
00659     /* Success */
00660   }
00661 
00668   void SickLD::DisableNearfieldSuppression( )
00669     throw( SickErrorException, SickTimeoutException, SickIOException ) {
00670 
00671     /* Ensure the device has been initialized */
00672     if (!_sick_initialized) {
00673       throw SickIOException("SickLD::DisableNearfieldSuppression: Device NOT Initialized!!!");
00674     }
00675   
00676     /* Tell the Sick LD to use nearfield suppression! */
00677     std::cout << "\tDisabling nearfield suppression..." << std::endl;
00678     try {
00679       _setSickFilter(SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF);
00680     }
00681 
00682     /* Handle a timeout! */
00683     catch (SickTimeoutException &sick_timeout_exception) {
00684       std::cerr << sick_timeout_exception.what() << std::endl;
00685       throw;
00686     }
00687     
00688     /* Handle I/O exceptions */
00689     catch (SickIOException &sick_io_exception) {
00690       std::cerr << sick_io_exception.what() << std::endl;
00691       throw;
00692     }
00693     
00694     /* Handle a returned error code */
00695     catch (SickErrorException &sick_error_exception) {
00696       std::cerr << sick_error_exception.what() << std::endl;
00697       throw;
00698     }
00699     
00700     /* A safety net */
00701     catch (...) {
00702       std::cerr << "SickLMS::DisableNearfieldSuppression: Unknown exception!!!" << std::endl;
00703       throw;
00704     }  
00705     
00706     std::cout << "\t\tSuppression is disabled!" << std::endl;
00707   
00708     /* Success */
00709   }
00710 
00740   void SickLD::GetSickMeasurements( double * const range_measurements,
00741                                     unsigned int * const echo_measurements,
00742                                     unsigned int * const num_measurements,
00743                                     unsigned int * const sector_ids,
00744                                     unsigned int * const sector_data_offsets,
00745                                     double * const sector_step_angles,
00746                                     double * const sector_start_angles,
00747                                     double * const sector_stop_angles,
00748                                     unsigned int * const sector_start_timestamps,
00749                                     unsigned int * const sector_stop_timestamps )
00750     throw( SickErrorException, SickIOException, SickTimeoutException, SickConfigException ){
00751 
00752     /* Ensure the device has been initialized */
00753     if(!_sick_initialized) {
00754       throw SickIOException("SickLD::GetSickMeasurements: Device NOT Initialized!!!");
00755     }
00756   
00757     /* The following conditional holds true if the user wants a RANGE+ECHO data
00758      * stream but already has an active RANGE-ONLY stream.
00759      */
00760     if (_sick_streaming_range_data && echo_measurements != NULL) {
00761 
00762       try {
00763         
00764         /* Cancel the current RANGE-ONLY data stream */
00765         _cancelSickScanProfiles();
00766 
00767         /* Request a RANGE+ECHO data stream */
00768         _getSickScanProfiles(SICK_SCAN_PROFILE_RANGE_AND_ECHO);
00769 
00770       }
00771 
00772       /* Handle a timeout! */
00773       catch (SickTimeoutException &sick_timeout_exception) {
00774         std::cerr << sick_timeout_exception.what() << std::endl;
00775         throw;
00776       }
00777       
00778       /* Handle I/O exceptions */
00779       catch (SickIOException &sick_io_exception) {
00780         std::cerr << sick_io_exception.what() << std::endl;
00781         throw;
00782       }
00783       
00784       /* Handle a returned error code */
00785       catch (SickErrorException &sick_error_exception) {
00786         std::cerr << sick_error_exception.what() << std::endl;
00787         throw;
00788       }
00789       
00790       /* A safety net */
00791       catch (...) {
00792         std::cerr << "SickLMS::GetSickMeasurements: Unknown exception!!!" << std::endl;
00793         throw;
00794       }  
00795       
00796     }
00797 
00798     /* The following conditional holds true if the user wants a RANGE-ONLY data
00799      * stream but already has an active RANGE+ECHO stream.
00800      */
00801     if (_sick_streaming_range_and_echo_data && echo_measurements == NULL) {
00802 
00803       try {
00804 
00805         /* Cancel the current RANGE+ECHO data stream */
00806         _cancelSickScanProfiles();
00807 
00808         /* Request a RANGE-ONLY data stream */
00809         _getSickScanProfiles(SICK_SCAN_PROFILE_RANGE);
00810         
00811       }
00812       
00813       /* Handle a timeout! */
00814       catch (SickTimeoutException &sick_timeout_exception) {
00815         std::cerr << sick_timeout_exception.what() << std::endl;
00816         throw;
00817       }
00818       
00819       /* Handle I/O exceptions */
00820       catch (SickIOException &sick_io_exception) {
00821         std::cerr << sick_io_exception.what() << std::endl;
00822         throw;
00823       }
00824       
00825       /* Handle a returned error code */
00826       catch (SickErrorException &sick_error_exception) {
00827         std::cerr << sick_error_exception.what() << std::endl;
00828         throw;
00829       }
00830       
00831       /* A safety net */
00832       catch (...) {
00833         std::cerr << "SickLMS::GetSickMeasurements: Unknown exception!!!" << std::endl;
00834         throw;
00835       }  
00836       
00837     }
00838 
00839     /* If there aren't any active data streams, setup a new one */
00840     if (!_sick_streaming_range_data && !_sick_streaming_range_and_echo_data) {
00841 
00842       try {
00843       
00844         /* Determine the target data stream by checking the value of echo_measurements */
00845         if (echo_measurements != NULL) {
00846           
00847           /* Request a RANGE+ECHO data stream */
00848           _getSickScanProfiles(SICK_SCAN_PROFILE_RANGE_AND_ECHO);         
00849           
00850         }
00851         else {
00852           
00853           /* Request a RANGE+ONLY data stream */
00854           _getSickScanProfiles(SICK_SCAN_PROFILE_RANGE);
00855           
00856         }
00857 
00858       }
00859 
00860       /* Handle a timeout! */
00861       catch (SickTimeoutException &sick_timeout_exception) {
00862         std::cerr << sick_timeout_exception.what() << std::endl;
00863         throw;
00864       }
00865       
00866       /* Handle I/O exceptions */
00867       catch (SickIOException &sick_io_exception) {
00868         std::cerr << sick_io_exception.what() << std::endl;
00869         throw;
00870       }
00871       
00872       /* Handle a returned error code */
00873       catch (SickErrorException &sick_error_exception) {
00874         std::cerr << sick_error_exception.what() << std::endl;
00875         throw;
00876       }
00877       
00878       /* A safety net */
00879       catch (...) {
00880         std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
00881         throw;
00882       }  
00883       
00884     }
00885 
00886     /* Declare the receive message object */
00887     SickLDMessage recv_message;
00888   
00889     /* Acquire the most recently buffered message */
00890     try {
00891       _recvMessage(recv_message,(unsigned int)1e6);
00892     }
00893     
00894     catch(SickTimeoutException &sick_timeout_exception) {
00895       std::cerr << sick_timeout_exception.what() << std::endl;
00896       throw;
00897     }  
00898 
00899     catch(...) {
00900       std::cerr << "SickLD::GetSickMeasurements - Unknown exception!" << std::endl;
00901       throw;
00902     }
00903     
00904     /* A single buffer for payload contents */
00905     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00906 
00907     /* Get the message payload */
00908     recv_message.GetPayload(payload_buffer);
00909 
00910     /* Define the destination Sick LD scan profile struct */
00911     sick_ld_scan_profile_t profile_data;
00912   
00913     /* Extract the scan profile */
00914     _parseScanProfile(&payload_buffer[2],profile_data);
00915 
00916     /* Update and check the returned sensor status */
00917     if ((_sick_sensor_mode = profile_data.sensor_status) != SICK_SENSOR_MODE_MEASURE) {
00918       throw SickConfigException("SickLD::GetSickMeasurements: Unexpected sensor mode! " + _sickSensorModeToString(_sick_sensor_mode));
00919     }
00920 
00921     /* Update and check the returned motor status */
00922     if ((_sick_motor_mode = profile_data.motor_status) != SICK_MOTOR_MODE_OK) {
00923       throw SickConfigException("SickLD::GetSickMeasurements: Unexpected motor mode! (Are you using a valid motor speed!)");
00924     }
00925 
00926     /* Everything is OK, so now populate the relevant return buffers */
00927     for (unsigned int i = 0, total_measurements = 0; i < _sick_sector_config.sick_num_active_sectors; i++) {
00928 
00929       /* Copy over the returned range values */
00930       memcpy(&range_measurements[total_measurements],profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].range_values,
00931              profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].num_data_points*sizeof(double));
00932     
00933       /* Copy the returned echo values  if requested */
00934       if (echo_measurements != NULL) {
00935         memcpy(&echo_measurements[total_measurements],profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].echo_values,
00936                profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].num_data_points*sizeof(unsigned int));
00937       }
00938     
00939       /* Set the number of measurements */
00940       if (num_measurements != NULL) {
00941         num_measurements[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].num_data_points;
00942       }
00943     
00944       /* Set the associated sector's id if requested */
00945       if (sector_ids != NULL) {
00946         sector_ids[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].sector_num;
00947       }
00948     
00949       /* Set the associated sector's index into the range measurement buffer if requested */
00950       if (sector_data_offsets != NULL) {
00951         sector_data_offsets[i] = total_measurements;
00952       }
00953     
00954       /* Set the step angle if requested */
00955       if (sector_step_angles != NULL) {
00956         sector_step_angles[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].angle_step;
00957       }
00958     
00959       /* Set the sector start angle if requested */
00960       if (sector_start_angles != NULL) {
00961         sector_start_angles[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].angle_start;
00962       }
00963     
00964       /* Set the sector stop angle if requested */
00965       if (sector_stop_angles != NULL) {
00966         sector_stop_angles[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].angle_stop;
00967       }
00968     
00969       /* Set the sector start timestamp if requested */
00970       if (sector_start_timestamps != NULL) {
00971         sector_start_timestamps[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].timestamp_start;
00972       }
00973     
00974       /* Set the sector stop timestamp if requested */
00975       if (sector_stop_timestamps != NULL) {
00976         sector_stop_timestamps[i] = profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].timestamp_stop;
00977       }
00978     
00979       /* Update the total number of measurements */
00980       total_measurements += profile_data.sector_data[_sick_sector_config.sick_active_sector_ids[i]].num_data_points;
00981     }
00982 
00983     /* Success */
00984   
00985   }
00986 
00991   void SickLD::SetSickSensorID( const unsigned int sick_sensor_id )
00992     throw( SickErrorException, SickTimeoutException, SickIOException ){
00993 
00994     /* Ensure the device has been initialized */
00995     if (!_sick_initialized) {
00996       throw SickConfigException("SickLD::SetSickSensorID: Device NOT Initialized!!!");
00997     }
00998 
00999     /* Check that the given ID is valid */
01000     if (!_validSickSensorID(sick_sensor_id)) {
01001       throw SickConfigException("SickLD::SetSickSensorID: Invalid sensor ID!!!");
01002     }
01003 
01004     /* Attempt to set the new sensor ID in the flash! */
01005     try {
01006       _setSickGlobalConfig(sick_sensor_id,GetSickMotorSpeed(),GetSickScanResolution());
01007     }
01008     
01009     /* Handle a timeout! */
01010     catch (SickTimeoutException &sick_timeout_exception) {
01011       std::cerr << sick_timeout_exception.what() << std::endl;
01012       throw;
01013     }
01014     
01015     /* Handle I/O exceptions */
01016     catch (SickIOException &sick_io_exception) {
01017       std::cerr << sick_io_exception.what() << std::endl;
01018       throw;
01019     }
01020     
01021     /* Handle a returned error code */
01022     catch (SickErrorException &sick_error_exception) {
01023       std::cerr << sick_error_exception.what() << std::endl;
01024       throw;
01025     }
01026     
01027     /* A safety net */
01028     catch (...) {
01029       std::cerr << "SickLMS::SetSickSensorID: Unknown exception!!!" << std::endl;
01030       throw;
01031     }  
01032   
01033     /* Success! */
01034   }
01035 
01040   void SickLD::SetSickMotorSpeed( const unsigned int sick_motor_speed )
01041     throw( SickErrorException, SickTimeoutException, SickIOException ) {
01042 
01043     /* Ensure the device has been initialized */
01044     if (!_sick_initialized) {
01045       throw SickIOException("SickLD::SetSickMotorSpeed: Device NOT Initialized!!!");
01046     }
01047 
01048     /* Check that the given motor speed is valid */
01049     if (!_validSickMotorSpeed(sick_motor_speed)) {
01050       throw SickConfigException("SickLD::SetSickMotorSpeed: Invalid sick motor speed!!!");
01051     }
01052     
01053     /* Check to ensure a valid pulse frequency for the device */
01054     if (!_validPulseFrequency(sick_motor_speed,GetSickScanResolution())) {
01055       throw SickConfigException("SickLD::SetSickMotorSpeed: Invalid pulse frequency!!!");
01056     }
01057     
01058     /* Attempt to set the new global config in the flash! */
01059     try {
01060       _setSickGlobalConfig(GetSickSensorID(),sick_motor_speed,GetSickScanResolution());
01061     }
01062     
01063     /* Handle a timeout! */
01064     catch (SickTimeoutException &sick_timeout_exception) {
01065       std::cerr << sick_timeout_exception.what() << std::endl;
01066       throw;
01067     }
01068     
01069     /* Handle I/O exceptions */
01070     catch (SickIOException &sick_io_exception) {
01071       std::cerr << sick_io_exception.what() << std::endl;
01072       throw;
01073     }
01074     
01075     /* Handle a returned error code */
01076     catch (SickErrorException &sick_error_exception) {
01077       std::cerr << sick_error_exception.what() << std::endl;
01078       throw;
01079     }
01080     
01081     /* A safety net */
01082     catch (...) {
01083       std::cerr << "SickLMS::SetSickMotorSpeed: Unknown exception!!!" << std::endl;
01084       throw;
01085     }      
01086   
01087     /* Success! */
01088   }
01089 
01102   void SickLD::SetSickScanResolution( const double sick_angle_step )
01103     throw( SickTimeoutException, SickIOException, SickConfigException ) {
01104 
01105     /* Ensure the device has been initialized */
01106     if (!_sick_initialized) {
01107       throw SickIOException("SickLD::SetSickScanResolution: Device NOT Initialized!!!");
01108     }
01109   
01110     /* Buffers to hold the active sector data */
01111     double active_sector_start_angles[SICK_MAX_NUM_SECTORS] = {0};
01112     double active_sector_stop_angles[SICK_MAX_NUM_SECTORS] = {0};
01113   
01114     /* Extract the start and stop angles for the current active sectors */
01115     for (unsigned int i = 0; i < _sick_sector_config.sick_num_active_sectors; i++) {
01116       active_sector_start_angles[i] = _sick_sector_config.sick_sector_start_angles[_sick_sector_config.sick_active_sector_ids[i]];
01117       active_sector_stop_angles[i] = _sick_sector_config.sick_sector_stop_angles[_sick_sector_config.sick_active_sector_ids[i]];   
01118     }
01119 
01120     /* Set the operating parameters accordingly */
01121     try { 
01122       SetSickGlobalParamsAndScanAreas(GetSickMotorSpeed(),sick_angle_step,
01123                                          active_sector_start_angles,active_sector_stop_angles,
01124                                          _sick_sector_config.sick_num_active_sectors);
01125     }
01126 
01127     catch(...) { }
01128   
01129     /* Success! */
01130   }
01131 
01140   void SickLD::SetSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed,
01141                                                 const double sick_angle_step,
01142                                                 const double * const active_sector_start_angles,
01143                                                 const double * const active_sector_stop_angles,
01144                                                 const unsigned int num_active_sectors )
01145     throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException ) {
01146 
01147     /* Ensure the device has been initialized */
01148     if (!_sick_initialized) {
01149       throw SickIOException("SickLD::SetSickGlobalParamsAndScanAreas: Device NOT Initialized!!!");
01150     }
01151   
01152     /* Attempt to write both a new scan resolution and scan area config */
01153     try {
01154       _setSickGlobalParamsAndScanAreas(sick_motor_speed,sick_angle_step,
01155                                        active_sector_start_angles,active_sector_stop_angles,num_active_sectors);    
01156     }
01157 
01158     /* Handle a timeout! */
01159     catch (SickTimeoutException &sick_timeout_exception) {
01160       std::cerr << sick_timeout_exception.what() << std::endl;
01161       throw;
01162     }
01163     
01164     /* Handle I/O exceptions */
01165     catch (SickIOException &sick_io_exception) {
01166       std::cerr << sick_io_exception.what() << std::endl;
01167       throw;
01168     }
01169 
01170     /* Handle config exception */
01171     catch (SickConfigException &sick_config_exception) {
01172       std::cerr << sick_config_exception.what() << std::endl;
01173       throw;
01174     }
01175     
01176     /* Handle a returned error code */
01177     catch (SickErrorException &sick_error_exception) {
01178       std::cerr << sick_error_exception.what() << std::endl;
01179       throw;
01180     }
01181     
01182     /* A safety net */
01183     catch (...) {
01184       std::cerr << "SickLMS::SetSickGlobalParamsAndScanAreas: Unknown exception!!!" << std::endl;
01185       throw;
01186     }  
01187   
01188     /* Success! */
01189   
01190   }
01191 
01198   void SickLD::SetSickScanAreas( const double * const active_sector_start_angles,
01199                                  const double * const active_sector_stop_angles,
01200                                  const unsigned int num_active_sectors )
01201     throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException ) {
01202 
01203     /* Ensure the device has been initialized */
01204     if (!_sick_initialized) {
01205       throw SickIOException("SickLD::SetSickScanAreas: Device NOT Initialized!!!");
01206     }
01207   
01208     /* Attempt to write both a new scan resolution and scan area config */
01209     try {
01210       _setSickGlobalParamsAndScanAreas(GetSickMotorSpeed(),GetSickScanResolution(),
01211                                           active_sector_start_angles,active_sector_stop_angles,num_active_sectors);
01212     }
01213 
01214     /* Handle a timeout! */
01215     catch (SickTimeoutException &sick_timeout_exception) {
01216       std::cerr << sick_timeout_exception.what() << std::endl;
01217       throw;
01218     }
01219     
01220     /* Handle I/O exceptions */
01221     catch (SickIOException &sick_io_exception) {
01222       std::cerr << sick_io_exception.what() << std::endl;
01223       throw;
01224     }
01225 
01226     /* Handle config exception */
01227     catch (SickConfigException &sick_config_exception) {
01228       std::cerr << sick_config_exception.what() << std::endl;
01229       throw;
01230     }
01231     
01232     /* Handle a returned error code */
01233     catch (SickErrorException &sick_error_exception) {
01234       std::cerr << sick_error_exception.what() << std::endl;
01235       throw;
01236     }
01237     
01238     /* A safety net */
01239     catch (...) {
01240       std::cerr << "SickLMS::SetSickScanAreas: Unknown exception!!!" << std::endl;
01241       throw;
01242     }  
01243     
01244     /* Success! */
01245   
01246   }
01247 
01252   void SickLD::ResetSick( const unsigned int reset_level )
01253     throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) {
01254 
01255     /* Ensure the device has been initialized */
01256     if (!_sick_initialized) {
01257       throw SickIOException("SickLD::ResetSick: Device NOT Initialized!!!");
01258     }
01259   
01260     /* Ensure a valid reset level was given */
01261     if (reset_level > SICK_WORK_SERV_RESET_HALT_APP) {
01262       throw SickConfigException("SickLD::ResetSick: Invalid given reset level!");
01263     }
01264   
01265     /* Allocate a single buffer for payload contents */
01266     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01267 
01268     /* Set the service IDs */
01269     payload_buffer[0] = SICK_WORK_SERV_CODE;             // Requested service type
01270     payload_buffer[1] = SICK_WORK_SERV_RESET;            // Requested service subtype
01271     payload_buffer[3] = (uint8_t)reset_level;            // RESETLEVEL
01272   
01273     /* Create the Sick messages */
01274     SickLDMessage send_message(payload_buffer,4);
01275     SickLDMessage recv_message;
01276 
01277     /* Send the message and check the reply */
01278     try {
01279       _sendMessageAndGetReply(send_message,recv_message);
01280     }
01281        
01282     /* Handle a timeout! */
01283     catch (SickTimeoutException &sick_timeout_exception) {
01284       std::cerr << sick_timeout_exception.what() << std::endl;
01285       throw;
01286     }
01287     
01288     /* Handle I/O exceptions */
01289     catch (SickIOException &sick_io_exception) {
01290       std::cerr << sick_io_exception.what() << std::endl;
01291       throw;
01292     }
01293     
01294     /* A safety net */
01295     catch (...) {
01296       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
01297       throw;
01298     }
01299 
01300     /* Reset the payload buffer */
01301     memset(payload_buffer,0,4);
01302   
01303     /* Acquire the returned payload */
01304     recv_message.GetPayload(payload_buffer);
01305 
01306     /* Extract the returned reset level */
01307     uint16_t current_reset_level;
01308     memcpy(&current_reset_level,&payload_buffer[2],2);
01309     current_reset_level = sick_ld_to_host_byte_order(current_reset_level);
01310 
01311     /* Verify the returned reset level */
01312     if (current_reset_level != (uint16_t)reset_level) {
01313       throw SickErrorException("SickLD::ResetSick: Unexpected returned reset level!");
01314     }
01315 
01316     /* Success */
01317   }
01318 
01323   unsigned int SickLD::GetSickNumActiveSectors( ) const {
01324     return _sick_sector_config.sick_num_active_sectors;
01325   }
01326 
01331   unsigned int SickLD::GetSickSensorID( ) const {
01332     return _sick_global_config.sick_sensor_id;
01333   }
01334 
01339   unsigned int SickLD::GetSickMotorSpeed( ) const {
01340     return _sick_global_config.sick_motor_speed;
01341   }
01342 
01347   double SickLD::GetSickScanResolution( ) const {
01348     return _sick_global_config.sick_angle_step;
01349   }
01350 
01355   std::string SickLD::GetSickIPAddress( ) const {
01356 
01357     /* Declare the string stream */
01358     std::ostringstream str_stream;
01359 
01360     str_stream << _sick_ethernet_config.sick_ip_address[0] << "."
01361                << _sick_ethernet_config.sick_ip_address[1] << "."
01362                << _sick_ethernet_config.sick_ip_address[2] << "."
01363                << _sick_ethernet_config.sick_ip_address[3];
01364 
01365     /* Return the std string representation */
01366     return str_stream.str();
01367 
01368   }
01369 
01374   std::string SickLD::GetSickSubnetMask( ) const {
01375 
01376     /* Declare the string stream */
01377     std::ostringstream str_stream;
01378 
01379     str_stream << _sick_ethernet_config.sick_subnet_mask[0] << "."
01380                << _sick_ethernet_config.sick_subnet_mask[1] << "."
01381                << _sick_ethernet_config.sick_subnet_mask[2] << "."
01382                << _sick_ethernet_config.sick_subnet_mask[3];
01383 
01384     /* Return the std string representation */
01385     return str_stream.str();
01386  
01387   }
01388 
01393   std::string SickLD::GetSickGatewayIPAddress( ) const {
01394 
01395     /* Declare the string stream */
01396     std::ostringstream str_stream;
01397 
01398     str_stream << _sick_ethernet_config.sick_gateway_ip_address[0] << "."
01399                << _sick_ethernet_config.sick_gateway_ip_address[1] << "."
01400                << _sick_ethernet_config.sick_gateway_ip_address[2] << "."
01401                << _sick_ethernet_config.sick_gateway_ip_address[3];
01402 
01403     /* Return the std string representation */
01404     return str_stream.str();
01405 
01406   }
01407 
01412   std::string SickLD::GetSickPartNumber( ) const {
01413     return _sick_identity.sick_part_number;
01414   }
01415 
01420   std::string SickLD::GetSickName( ) const {
01421     return _sick_identity.sick_name;
01422   }
01423 
01428   std::string SickLD::GetSickVersion( ) const {
01429     return _sick_identity.sick_version;
01430   }
01431 
01436   std::string SickLD::GetSickSerialNumber( ) const {
01437     return _sick_identity.sick_serial_number;
01438   }
01439 
01444   std::string SickLD::GetSickEDMSerialNumber( ) const {
01445     return _sick_identity.sick_edm_serial_number;
01446   }
01447 
01452   std::string SickLD::GetSickFirmwarePartNumber( ) const {
01453     return _sick_identity.sick_firmware_part_number;
01454   }
01455 
01460   std::string SickLD::GetSickFirmwareName( ) const {
01461     return _sick_identity.sick_firmware_name;
01462   }
01463 
01468   std::string SickLD::GetSickFirmwareVersion( ) const {
01469     return _sick_identity.sick_firmware_version;
01470   }
01471 
01476   std::string SickLD::GetSickAppSoftwarePartNumber( ) const {
01477     return _sick_identity.sick_application_software_part_number;
01478   }
01479 
01484   std::string SickLD::GetSickAppSoftwareName( ) const {
01485     return _sick_identity.sick_application_software_name;
01486   }
01487 
01492   std::string SickLD::GetSickAppSoftwareVersionNumber( ) const {
01493     return _sick_identity.sick_application_software_version;
01494   }
01495 
01500   std::string SickLD::GetSickStatusAsString() const {
01501 
01502     std::stringstream str_stream;
01503 
01504     str_stream << "\t============= Sick LD Status =============" << std::endl;
01505     str_stream << "\tSensor Mode: " << _sickSensorModeToString(_sick_sensor_mode) << std::endl;
01506     str_stream << "\tMotor Mode: " << _sickMotorModeToString(_sick_motor_mode) << std::endl;
01507     str_stream << "\t==========================================" << std::endl;    
01508 
01509     return str_stream.str();
01510     
01511   }
01512   
01517   std::string SickLD::GetSickIdentityAsString() const {
01518 
01519     std::ostringstream str_stream;
01520     
01521     str_stream << "\t============ Sick LD Identity ============" << std::endl;
01522     str_stream << "\tSensor Part #: " << GetSickPartNumber() << std::endl;
01523     str_stream << "\tSensor Name: " << GetSickName() << std::endl;
01524     str_stream << "\tSensor Version: " << GetSickVersion() << std::endl;
01525     str_stream << "\tSensor Serial #: " << GetSickSerialNumber() << std::endl;
01526     str_stream << "\tSensor EDM Serial #: " << GetSickEDMSerialNumber() << std::endl;
01527     str_stream << "\tFirmware Part #: " << GetSickFirmwarePartNumber() << std::endl;
01528     str_stream << "\tFirmware Version: " << GetSickFirmwareVersion() << std::endl;
01529     str_stream << "\tFirmware Name: " << GetSickFirmwareName() << std::endl;
01530     str_stream << "\tApp. Software Part #: " << GetSickAppSoftwarePartNumber() << std::endl;
01531     str_stream << "\tApp. Software Name: " << GetSickAppSoftwareName() << std::endl;
01532     str_stream << "\tApp. Software Version: " << GetSickAppSoftwareVersionNumber() << std::endl;
01533     str_stream << "\t==========================================" << std::endl;
01534     
01535     return str_stream.str();
01536 
01537   }
01538   
01543   std::string SickLD::GetSickGlobalConfigAsString() const {
01544 
01545     std::stringstream str_stream;
01546     
01547     str_stream << "\t=========== Sick Global Config ===========" << std::endl;
01548     str_stream << "\tSensor ID: " << GetSickSensorID() << std::endl;
01549     str_stream << "\tMotor Speed (5 to 20Hz): " << GetSickMotorSpeed() << std::endl;
01550     str_stream << "\tAngle Step (deg): " << GetSickScanResolution() << std::endl;
01551     str_stream << "\t==========================================" << std::endl;
01552     
01553     return str_stream.str();
01554     
01555   }
01556 
01561   std::string SickLD::GetSickEthernetConfigAsString() const {
01562 
01563     std::stringstream str_stream;
01564 
01565     str_stream << "\t========== Sick Ethernet Config ==========" << std::endl;
01566     str_stream << "\tIP Address: " << GetSickIPAddress() << std::endl;
01567     str_stream << "\tSubnet Mask: " << GetSickSubnetMask() << std::endl;
01568     str_stream << "\tGateway IP Address: " << GetSickGatewayIPAddress() << std::endl;
01569     str_stream << "\t==========================================" << std::endl;
01570     
01571     return str_stream.str();
01572     
01573   }
01574   
01579   std::string SickLD::GetSickSectorConfigAsString() const {
01580 
01581     std::stringstream str_stream;
01582     
01583     str_stream << "\t=========== Sick Sector Config ===========" << std::endl;
01584     str_stream << "\tNum. Active Sectors: " << (int)_sick_sector_config.sick_num_active_sectors << std::endl;
01585     str_stream << "\tNum. Initialized Sectors: " << (int)_sick_sector_config.sick_num_initialized_sectors << std::endl;
01586     
01587     str_stream << "\tSector Configs.:" << std::endl;
01588     for (unsigned int i = 0; i < _sick_sector_config.sick_num_initialized_sectors; i++) {
01589       str_stream << "\t\t" << i << " ["
01590                  << _sick_sector_config.sick_sector_start_angles[i] << ","
01591                  << _sick_sector_config.sick_sector_stop_angles[i] << "] ("
01592                  << _sickSectorFunctionToString(_sick_sector_config.sick_sector_functions[i]) << ")" << std::endl;
01593     }
01594     
01595     str_stream << "\t==========================================" << std::endl;
01596     
01597     return str_stream.str();
01598 
01599   }
01600   
01605   double SickLD::GetSickScanArea( ) const {
01606 
01607     /* Some temp buffers */
01608     double sector_start_angles[SICK_MAX_NUM_SECTORS] = {0};
01609     double sector_stop_angles[SICK_MAX_NUM_SECTORS] = {0};
01610   
01611     /* Sum the active areas over all sectors */
01612     for (unsigned int i = 0; i < _sick_sector_config.sick_num_active_sectors; i++) {
01613       sector_start_angles[i] = _sick_sector_config.sick_sector_start_angles[_sick_sector_config.sick_active_sector_ids[i]];
01614       sector_stop_angles[i] = _sick_sector_config.sick_sector_stop_angles[_sick_sector_config.sick_active_sector_ids[i]];    
01615     }  
01616   
01617     /* Return the computed total scan area */
01618     return _computeScanArea(GetSickScanResolution(),sector_start_angles,sector_stop_angles,_sick_sector_config.sick_num_active_sectors);
01619   }
01620 
01624   void SickLD::PrintSickStatus( ) const {  
01625     std::cout << GetSickStatusAsString() << std::flush;
01626   }
01627 
01631   void SickLD::PrintSickIdentity( ) const {
01632     std::cout << GetSickIdentityAsString() << std::flush;
01633   }
01634 
01638   void SickLD::PrintSickGlobalConfig( ) const {  
01639     std::cout << GetSickGlobalConfigAsString() << std::flush;
01640   }
01641 
01645   void SickLD::PrintSickEthernetConfig( ) const {  
01646     std::cout << GetSickEthernetConfigAsString() << std::flush;
01647   }
01648 
01652   void SickLD::PrintSickSectorConfig( ) const {  
01653     std::cout << GetSickSectorConfigAsString() << std::flush;
01654   }
01655 
01659   void SickLD::Uninitialize( ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException ){
01660 
01661     /* Ensure the device has been initialized */
01662     if (!_sick_initialized) {
01663       throw SickIOException("SickLD::Uninitialize: Device NOT Initialized!!!");
01664     }
01665 
01666     std::cout << std::endl << "\t*** Attempting to uninitialize the Sick LD..." << std::endl; 
01667   
01668     /* If necessary, tell the Sick LD to stop streaming data */
01669     try {
01670       
01671       std::cout << "\tSetting Sick LD to idle mode..." << std::endl;
01672       _setSickSensorModeToIdle();
01673       std::cout << "\t\tSick LD is now idle!" << std::endl;
01674 
01675       /* Clear any signals that were set */
01676       SetSickSignals();
01677 
01678       /* Attempt to cancel the buffer monitor */
01679       std::cout << "\tAttempting to cancel buffer monitor..." << std::endl;
01680       _stopListening();
01681       std::cout << "\t\tBuffer monitor canceled!" << std::endl;
01682     
01683       /* Attempt to close the tcp connection */
01684       std::cout << "\tClosing connection to Sick LD..." << std::endl;
01685       _teardownConnection();
01686 
01687     }
01688            
01689     /* Handle a timeout! */
01690     catch (SickTimeoutException &sick_timeout_exception) {
01691       std::cerr << sick_timeout_exception.what() << std::endl;
01692       throw;
01693     }
01694     
01695     /* Handle I/O exceptions */
01696     catch (SickIOException &sick_io_exception) {
01697       std::cerr << sick_io_exception.what() << std::endl;
01698       throw;
01699     }
01700     
01701     /* Handle a returned error code */
01702     catch (SickErrorException &sick_error_exception) {
01703       std::cerr << sick_error_exception.what() << std::endl;
01704       throw;
01705     }
01706 
01707     /* Handle a returned error code */
01708     catch (SickThreadException &sick_thread_exception) {
01709       std::cerr << sick_thread_exception.what() << std::endl;
01710       throw;
01711     }
01712     
01713     /* A safety net */
01714     catch (...) {
01715       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
01716       throw;
01717     }  
01718 
01719     std::cout << "\t\tConnection closed!" << std::endl;
01720 
01721     std::cout << "\t*** Uninit. complete - Sick LD is now offline!" << std::endl; 
01722 
01723     /* Mark the device as uninitialized */
01724     _sick_initialized = false;
01725   
01726     /* Success! */
01727   }
01728 
01732   void SickLD::_setupConnection( ) throw( SickIOException, SickTimeoutException ) {
01733 
01734     /* Create the TCP socket */
01735     if ((_sick_fd = socket(PF_INET,SOCK_STREAM,IPPROTO_TCP)) < 0) {
01736       throw SickIOException("SickLD::_setupConnection: socket() failed!");
01737     }
01738 
01739     /* Initialize the buffer */
01740     memset(&_sick_inet_address_info,0,sizeof(struct sockaddr_in));
01741   
01742     /* Setup the Sick LD inet address structure */
01743     _sick_inet_address_info.sin_family = AF_INET;                                  // Internet protocol address family
01744     _sick_inet_address_info.sin_port = htons(_sick_tcp_port);                      // TCP port number
01745     _sick_inet_address_info.sin_addr.s_addr = inet_addr(_sick_ip_address.c_str()); // Convert ip string to numerical address
01746 
01747     try {
01748 
01749       /* Set to non-blocking so we can time connect */
01750       _setNonBlockingIO();
01751     
01752       /* Try to connect to the Sick LD */
01753       int conn_return;
01754       if ((conn_return = connect(_sick_fd,(struct sockaddr *)&_sick_inet_address_info,sizeof(struct sockaddr_in))) < 0) {
01755 
01756         /* Check whether it is b/c it would block */
01757         if (errno != EINPROGRESS) {     
01758           throw SickIOException("SickLD::_setupConnection: connect() failed!");
01759         }
01760 
01761         /* Use select to wait on the socket */
01762         int valid_opt = 0;
01763         int num_active_files = 0;
01764         struct timeval timeout_val;                          // This structure will be used for setting our timeout values
01765         fd_set file_desc_set;                                // File descriptor set for monitoring I/O
01766     
01767         /* Initialize and set the file descriptor set for select */
01768         FD_ZERO(&file_desc_set);
01769         FD_SET(_sick_fd,&file_desc_set);
01770 
01771         /* Setup the timeout structure */
01772         memset(&timeout_val,0,sizeof(timeout_val));          // Initialize the buffer
01773         timeout_val.tv_usec = DEFAULT_SICK_CONNECT_TIMEOUT;  // Wait for specified time before throwing a timeout
01774       
01775         /* Wait for the OS to tell us that the connection is established! */
01776         num_active_files = select(getdtablesize(),0,&file_desc_set,0,&timeout_val);
01777       
01778         /* Figure out what to do based on the output of select */
01779         if (num_active_files > 0) {
01780         
01781           /* This is just a sanity check */
01782           if (!FD_ISSET(_sick_fd,&file_desc_set)) {
01783             throw SickIOException("SickLD::_setupConnection: Unexpected file descriptor!");
01784           }       
01785 
01786           /* Check for any errors on the socket - just to be sure */
01787           socklen_t len = sizeof(int);
01788           if (getsockopt(_sick_fd,SOL_SOCKET,SO_ERROR,(void*)(&valid_opt),&len) < 0) {      
01789             throw SickIOException("SickLD::_setupConnection: getsockopt() failed!");
01790           } 
01791 
01792           /* Check whether the opt value indicates error */
01793           if (valid_opt) { 
01794             throw SickIOException("SickLD::_setupConnection: socket error on connect()!");
01795           }
01796           
01797         }
01798         else if (num_active_files == 0) {
01799         
01800           /* A timeout has occurred! */
01801           throw SickTimeoutException("SickLD::_setupConnection: select() timeout!");    
01802 
01803         }
01804         else {
01805         
01806           /* An error has occurred! */
01807           throw SickIOException("SickLD::_setupConnection: select() failed!");  
01808 
01809         }
01810 
01811       }
01812 
01813       /* Restore blocking IO */
01814       _setBlockingIO(); 
01815         
01816     }
01817 
01818     catch(SickIOException &sick_io_exception) {
01819       std::cerr << sick_io_exception.what() << std::endl;
01820       throw;
01821     }
01822 
01823     catch(SickTimeoutException &sick_timeout_exception) {
01824       std::cerr << sick_timeout_exception.what() << std::endl;
01825       throw;
01826     }
01827     
01828     catch(...) {
01829       std::cerr << "SickLD::_setupConnection - Unknown exception occurred!" << std::endl;
01830       throw;
01831     }
01832 
01833     /* Success */
01834   }
01835 
01841   void SickLD::_syncDriverWithSick( )  throw( SickIOException, SickTimeoutException, SickErrorException ) {
01842 
01843     try {
01844       
01845       /* Acquire current configuration */
01846       _getSickStatus();
01847       _getSickIdentity();
01848       _getSickEthernetConfig();
01849       _getSickGlobalConfig();
01850       _getSickSectorConfig();
01851 
01852       /* Reset Sick signals */
01853       _setSickSignals();
01854 
01855     }
01856 
01857     /* Handle a timeout! */
01858     catch (SickTimeoutException &sick_timeout_exception) {
01859       std::cerr << sick_timeout_exception.what() << std::endl;
01860       throw;
01861     }
01862     
01863     /* Handle I/O exceptions */
01864     catch (SickIOException &sick_io_exception) {
01865       std::cerr << sick_io_exception.what() << std::endl;
01866       throw;
01867     }
01868     
01869     /* Handle a returned error code */
01870     catch (SickErrorException &sick_error_exception) {
01871       std::cerr << sick_error_exception.what() << std::endl;
01872       throw;
01873     }
01874     
01875     /* A safety net */
01876     catch (...) {
01877       std::cerr << "SickLMS::_syncDriverWithSick: Unknown exception!!!" << std::endl;
01878       throw;
01879     }  
01880   
01881     /* Success */
01882   }
01883 
01890   void SickLD::_setSickSectorFunction ( const uint8_t sector_number, const uint8_t sector_function,
01891                                         const double sector_stop_angle, const bool write_to_flash )
01892     throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) {
01893 
01894     /* Ensure the device is not measuring */
01895     if (_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) {
01896 
01897       /* Set the Sick LD to rotate mode */
01898       try {
01899         _setSickSensorModeToRotate();
01900       }
01901 
01902       /* Handle a timeout! */
01903       catch (SickTimeoutException &sick_timeout_exception) {
01904         std::cerr << sick_timeout_exception.what() << std::endl;
01905         throw;
01906       }
01907       
01908       /* Handle I/O exceptions */
01909       catch (SickIOException &sick_io_exception) {
01910         std::cerr << sick_io_exception.what() << std::endl;
01911         throw;
01912       }
01913       
01914       /* Handle a returned error code */
01915       catch (SickErrorException &sick_error_exception) {
01916         std::cerr << sick_error_exception.what() << std::endl;
01917         throw;
01918       }
01919       
01920       /* A safety net */
01921       catch (...) {
01922         std::cerr << "SickLMS::_setSickSectorFunction: Unknown exception!!!" << std::endl;
01923         throw;
01924       }  
01925       
01926     }
01927   
01928     /* Ensure a valid sector number */
01929     if (sector_number >= SICK_MAX_NUM_SECTORS) {
01930       throw SickConfigException("SickLD::_setSickSectorFunction: Invalid sector number!");
01931     }
01932 
01933     /* Check that a valid sector_function was given */
01934     if (sector_function != SICK_CONF_SECTOR_NOT_INITIALIZED &&
01935         sector_function != SICK_CONF_SECTOR_NO_MEASUREMENT &&
01936         sector_function != SICK_CONF_SECTOR_RESERVED &&
01937         sector_function != SICK_CONF_SECTOR_NORMAL_MEASUREMENT &&
01938         sector_function != SICK_CONF_SECTOR_REFERENCE_MEASUREMENT) {
01939 
01940       throw SickConfigException("SickLD::_setSickSectorFunction: Invalid sector function code!");
01941     }
01942 
01943     /* Check that a valid stop angle was given */
01944     if (sector_stop_angle > SICK_MAX_SCAN_AREA) {
01945       throw SickConfigException("SickLD::_setSickSectorFunction: Invalid sector stop angle!");
01946     }
01947   
01948     /* Allocate a single buffer for payload contents */
01949     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01950 
01951     /* A temporary buffer for byte order conversion */
01952     uint16_t temp_buff = 0;
01953   
01954     /* Set the service IDs */
01955     payload_buffer[0] = SICK_CONF_SERV_CODE;          // Requested service type
01956     payload_buffer[1] = SICK_CONF_SERV_SET_FUNCTION;  // Requested service subtype                  
01957 
01958     /* Assign the payload data */
01959     payload_buffer[3] = sector_number;                // SECTORNUM
01960     payload_buffer[5] = sector_function;              // SECTORFUNC
01961 
01962     /* Set the sector stop value */
01963     temp_buff = host_to_sick_ld_byte_order(_angleToTicks(sector_stop_angle));    
01964     memcpy(&payload_buffer[6],&temp_buff,2);          // SECTORSTOP
01965 
01966     /* Include the flash flag */
01967     payload_buffer[9] = (uint8_t)write_to_flash;      // FLASHFLAG
01968 
01969     /* Create the Sick LD messages */
01970     SickLDMessage send_message(payload_buffer,10);    
01971     SickLDMessage recv_message;
01972 
01973     /* Send the message and get the reply */
01974     try {
01975       _sendMessageAndGetReply(send_message,recv_message);
01976     }
01977        
01978     /* Handle a timeout! */
01979     catch (SickTimeoutException &sick_timeout_exception) {
01980       std::cerr << sick_timeout_exception.what() << std::endl;
01981       throw;
01982     }
01983     
01984     /* Handle I/O exceptions */
01985     catch (SickIOException &sick_io_exception) {
01986       std::cerr << sick_io_exception.what() << std::endl;
01987       throw;
01988     }
01989     
01990     /* A safety net */
01991     catch (...) {
01992       std::cerr << "SickLMS::_setSickSectorFunction: Unknown exception!!!" << std::endl;
01993       throw;
01994     }
01995 
01996     /* Reset the payload buffer (not necessary, but it doesn't hurt to be careful) */
01997     memset(payload_buffer,0,10);
01998   
01999     /* Extract the message payload */
02000     recv_message.GetPayload(payload_buffer);  
02001 
02002     /* Check the response for an error */
02003     if (payload_buffer[2] == 0xFF && payload_buffer[3] == 0xFF) {
02004       throw SickConfigException("SickLD::_setSickSectorFunction: Invalid request!");
02005     }
02006   
02007     /* Success! */
02008   }
02009 
02016   void SickLD::_getSickSectorFunction( const uint8_t sector_num, uint8_t &sector_function, double &sector_stop_angle )
02017     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02018 
02019     /* Ensure the device is not measuring */
02020     if (_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) {
02021 
02022       /* Set the Sick LD to rotate mode */
02023       try {
02024         _setSickSensorModeToRotate();
02025       }
02026 
02027       /* Handle a timeout! */
02028       catch (SickTimeoutException &sick_timeout_exception) {
02029         std::cerr << sick_timeout_exception.what() << std::endl;
02030         throw;
02031       }
02032       
02033       /* Handle I/O exceptions */
02034       catch (SickIOException &sick_io_exception) {
02035         std::cerr << sick_io_exception.what() << std::endl;
02036         throw;
02037       }
02038       
02039       /* Handle a returned error code */
02040       catch (SickErrorException &sick_error_exception) {
02041         std::cerr << sick_error_exception.what() << std::endl;
02042         throw;
02043       }
02044       
02045       /* A safety net */
02046       catch (...) {
02047         std::cerr << "SickLMS::_getSickSectorFunction: Unknown exception!!!" << std::endl;
02048         throw;
02049       }  
02050       
02051     }
02052   
02053     /* Declare the message payload buffer */
02054     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02055 
02056     /* Set the service IDs */
02057     payload_buffer[0] = SICK_CONF_SERV_CODE;            // Requested service type
02058     payload_buffer[1] = SICK_CONF_SERV_GET_FUNCTION;    // Requested service subtype
02059     payload_buffer[3] = sector_num;                     // Sector number
02060     
02061     /* Declare the send/recv Sick LD message objects */
02062     SickLDMessage send_message(payload_buffer,4);
02063     SickLDMessage recv_message;
02064 
02065     /* Send the message and get a response */
02066     try {
02067       _sendMessageAndGetReply(send_message,recv_message);
02068     }
02069             
02070     /* Handle a timeout! */
02071     catch (SickTimeoutException &sick_timeout_exception) {
02072       std::cerr << sick_timeout_exception.what() << std::endl;
02073       throw;
02074     }
02075     
02076     /* Handle I/O exceptions */
02077     catch (SickIOException &sick_io_exception) {
02078       std::cerr << sick_io_exception.what() << std::endl;
02079       throw;
02080     }
02081     
02082     /* A safety net */
02083     catch (...) {
02084       std::cerr << "SickLMS::_getSickSectorFunction: Unknown exception!!!" << std::endl;
02085       throw;
02086     }
02087 
02088     /* Reset the payload buffer */
02089     memset(payload_buffer,0,4);
02090   
02091     /* Extract the message payload */
02092     recv_message.GetPayload(payload_buffer);
02093 
02094     /* Extract the returned sector number */
02095     uint16_t temp_buffer = 0;
02096     memcpy(&temp_buffer,&payload_buffer[2],2);
02097     temp_buffer = sick_ld_to_host_byte_order(temp_buffer);
02098 
02099     /* Check to make sure the returned sector number matches
02100      * the requested sector number.
02101      */
02102     if (temp_buffer != sector_num) {
02103       throw SickConfigException("SickLD::_getSickSectorFunction: Unexpected sector number returned by Sick LD!");
02104     }
02105 
02106     /* Extract the sector function */
02107     memcpy(&temp_buffer,&payload_buffer[4],2);
02108     sector_function = sick_ld_to_host_byte_order(temp_buffer);
02109 
02110     /* Extract the sector stop angle (in ticks) */
02111     memcpy(&temp_buffer,&payload_buffer[6],2);
02112     sector_stop_angle = _ticksToAngle(sick_ld_to_host_byte_order(temp_buffer));
02113   
02114     /* S'ok */
02115   }
02116 
02120   void SickLD::_setSickSensorModeToIdle( )
02121     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02122 
02123     /* If necessary adjust the operating mode of the sensor */
02124     if (_sick_sensor_mode != SICK_SENSOR_MODE_IDLE) {
02125     
02126       /* Switch the sensor's operating mode to IDLE */
02127       try {
02128         _setSickSensorMode(SICK_SENSOR_MODE_IDLE);
02129       }
02130       
02131       /* Handle a timeout! */
02132       catch (SickTimeoutException &sick_timeout_exception) {
02133         std::cerr << sick_timeout_exception.what() << std::endl;
02134         throw;
02135       }
02136       
02137       /* Handle I/O exceptions */
02138       catch (SickIOException &sick_io_exception) {
02139         std::cerr << sick_io_exception.what() << std::endl;
02140         throw;
02141       }
02142 
02143       /* Handle a returned error code */
02144       catch (SickErrorException &sick_error_exception) {
02145         std::cerr << sick_error_exception.what() << std::endl;
02146         throw;
02147       }
02148       
02149       /* A safety net */
02150       catch (...) {
02151         std::cerr << "SickLMS::_setSickSensorModeToIdle: Unknown exception!!!" << std::endl;
02152         throw;
02153       }  
02154       
02155     }
02156     
02157     /* Success */
02158   }
02159 
02163   void SickLD::_setSickSensorModeToRotate( )
02164     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02165 
02166     /* If necessary adjust the operating mode of the sensor */
02167     if (_sick_sensor_mode != SICK_SENSOR_MODE_ROTATE) {
02168     
02169       /* Switch the sensor's operating mode to ROTATE */
02170       try {
02171         _setSickSensorMode(SICK_SENSOR_MODE_ROTATE);
02172       }
02173 
02174       /* Handle a timeout! */
02175       catch (SickTimeoutException &sick_timeout_exception) {
02176         std::cerr << sick_timeout_exception.what() << std::endl;
02177         throw;
02178       }
02179       
02180       /* Handle I/O exceptions */
02181       catch (SickIOException &sick_io_exception) {
02182         std::cerr << sick_io_exception.what() << std::endl;
02183         throw;
02184       }
02185 
02186       /* Handle a returned error code */
02187       catch (SickErrorException &sick_error_exception) {
02188         std::cerr << sick_error_exception.what() << std::endl;
02189         throw;
02190       }
02191       
02192       /* A safety net */
02193       catch (...) {
02194         std::cerr << "SickLMS::_setSickSensorModeToRotate: Unknown exception!!!" << std::endl;
02195         throw;
02196       }  
02197     
02198     }
02199   
02200     /* Success */
02201   }
02202 
02206   void SickLD::_setSickSensorModeToMeasure( ) 
02207     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02208 
02209     /* If necessary adjust the operating mode of the sensor */
02210     if (_sick_sensor_mode != SICK_SENSOR_MODE_MEASURE) {
02211     
02212       /* Switch the sensor's operating mode to MEASURE */
02213       try {
02214         _setSickSensorMode(SICK_SENSOR_MODE_MEASURE);
02215       }
02216 
02217       /* Handle a timeout! */
02218       catch (SickTimeoutException &sick_timeout_exception) {
02219         std::cerr << sick_timeout_exception.what() << std::endl;
02220         throw;
02221       }
02222       
02223       /* Handle I/O exceptions */
02224       catch (SickIOException &sick_io_exception) {
02225         std::cerr << sick_io_exception.what() << std::endl;
02226         throw;
02227       }
02228 
02229       /* Handle a returned error code */
02230       catch (SickErrorException &sick_error_exception) {
02231         std::cerr << sick_error_exception.what() << std::endl;
02232         throw;
02233       }
02234       
02235       /* A safety net */
02236       catch (...) {
02237         std::cerr << "SickLMS::_setSickSensorModeToMeasure: Unknown exception!!!" << std::endl;
02238         throw;
02239       }  
02240     
02241     }
02242   
02243     /* Success */
02244   }
02245 
02250   void SickLD::_setSickSensorMode( const uint8_t new_sick_sensor_mode ) 
02251     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02252   
02253     /* If the new mode matches the current mode then just return */
02254     if (_sick_sensor_mode == new_sick_sensor_mode) {
02255       return;
02256     }
02257 
02258     try {
02259     
02260       /* If the current sensor mode is MEASURE and streaming data */
02261       if ((_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) &&
02262           (_sick_streaming_range_data || _sick_streaming_range_and_echo_data)) {
02263         
02264         /* Cancel the current stream */
02265         _cancelSickScanProfiles();
02266         
02267       }
02268       
02269       /* The Sick LD must be in rotate mode before: going from IDLE to MEASURE or going from MEASURE to IDLE */
02270       if ((_sick_sensor_mode == SICK_SENSOR_MODE_IDLE && new_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) ||
02271           (_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE && new_sick_sensor_mode == SICK_SENSOR_MODE_IDLE)) {
02272         
02273         /* Set to rotate mode */
02274         _setSickSensorModeToRotate();
02275         
02276       }
02277 
02278     }
02279           
02280     /* Handle a timeout! */
02281     catch (SickTimeoutException &sick_timeout_exception) {
02282       std::cerr << sick_timeout_exception.what() << std::endl;
02283       throw;
02284     }
02285     
02286     /* Handle I/O exceptions */
02287     catch (SickIOException &sick_io_exception) {
02288       std::cerr << sick_io_exception.what() << std::endl;
02289       throw;
02290     }
02291     
02292     /* Handle a returned error code */
02293     catch (SickErrorException &sick_error_exception) {
02294       std::cerr << sick_error_exception.what() << std::endl;
02295       throw;
02296     }
02297     
02298     /* A safety net */
02299     catch (...) {
02300       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
02301       throw;
02302     }  
02303     
02304     /* Allocate a single buffer for payload contents */
02305     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02306     
02307     /* The payload length */
02308     uint32_t payload_length = 2;
02309     
02310     /* Set the service IDs */
02311     payload_buffer[0] = SICK_WORK_SERV_CODE;                                       // Requested service type
02312     payload_buffer[1] = _sickSensorModeToWorkServiceSubcode(new_sick_sensor_mode); // Requested service subtype
02313     
02314     /* If the target sensor mode is rotate then we add two more bytes
02315      * to the payload length. Doing so adds two zero values to the payload
02316      * which tells it to use the angular step and scan freqeuncy values
02317      * stored in its flash.
02318      */
02319     if (new_sick_sensor_mode == SICK_SENSOR_MODE_ROTATE) {
02320       payload_length += 2;
02321     }
02322     
02323     /* Define the send/receive message objects */
02324     SickLDMessage send_message(payload_buffer,payload_length);
02325     SickLDMessage recv_message;
02326 
02327     try {
02328       _sendMessageAndGetReply(send_message,recv_message);
02329     }
02330         
02331     /* Handle a timeout! */
02332     catch (SickTimeoutException &sick_timeout_exception) {
02333       std::cerr << sick_timeout_exception.what() << std::endl;
02334       throw;
02335     }
02336     
02337     /* Handle I/O exceptions */
02338     catch (SickIOException &sick_io_exception) {
02339       std::cerr << sick_io_exception.what() << std::endl;
02340       throw;
02341     }
02342     
02343     /* A safety net */
02344     catch (...) {
02345       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
02346       throw;
02347     }
02348       
02349     /* Reset the payload buffer */
02350     memset(payload_buffer,0,payload_length);
02351   
02352     /* Extract the message payload */
02353     recv_message.GetPayload(payload_buffer);
02354 
02355     /* Ensure the returned mode matches the requested mode */
02356     if ((_sick_sensor_mode = (payload_buffer[5] & 0x0F)) != new_sick_sensor_mode) {
02357 
02358       /* Check whether there is an error code we can use */
02359       if (new_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) {
02360 
02361         uint16_t return_code = 0;
02362         std::string errMsg = "SickLD::_setSickSensorMode: Unexpected sensor mode returned from Sick LD!";
02363         memcpy(&return_code,&payload_buffer[6],2);
02364         return_code = sick_ld_to_host_byte_order(return_code);
02365 
02366         /* Print the error code associated with the TRANS_MEASURE request */
02367         errMsg = errMsg + " (TRANS_MEAS Error Code: " + _sickTransMeasureReturnToString(return_code) + ")";
02368         throw SickErrorException(errMsg.c_str());
02369         
02370       }
02371 
02372     }
02373 
02374     /* Make sure the motor is Ok */
02375     if ((_sick_motor_mode = ((payload_buffer[5] >> 4) & 0x0F)) != SICK_MOTOR_MODE_OK) {
02376       throw SickErrorException("SickLD::_setSickSensorMode: Unexpected motor mode returned from Sick LD!");
02377     }
02378 
02379     /* Success */
02380 
02381   }
02382 
02389   void SickLD::_getSickScanProfiles( const uint16_t profile_format, const uint16_t num_profiles )
02390     throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) {
02391 
02392     /* Ensure the device is in measurement mode */
02393     try {
02394       _setSickSensorModeToMeasure();
02395     }
02396        
02397     /* Handle a timeout! */
02398     catch (SickTimeoutException &sick_timeout_exception) {
02399       std::cerr << sick_timeout_exception.what() << std::endl;
02400       throw;
02401     }
02402     
02403     /* Handle I/O exceptions */
02404     catch (SickIOException &sick_io_exception) {
02405       std::cerr << sick_io_exception.what() << std::endl;
02406       throw;
02407     }
02408     
02409     /* Handle a returned error code */
02410     catch (SickErrorException &sick_error_exception) {
02411       std::cerr << sick_error_exception.what() << std::endl;
02412       throw;
02413     }
02414     
02415     /* A safety net */
02416     catch (...) {
02417       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
02418       throw;
02419     }  
02420   
02421     /* A quick check to ensure the requested format is supported by the driver */
02422     if (!_supportedScanProfileFormat(profile_format)) {
02423       throw SickConfigException("SickLD::_getSickScanProfiles: Unsupported profile format!");
02424     }
02425   
02426     /* Allocate a single buffer for payload contents */
02427     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02428 
02429     /* Set the service code and subcode */
02430     payload_buffer[0] = SICK_MEAS_SERV_CODE;
02431     payload_buffer[1] = SICK_MEAS_SERV_GET_PROFILE;
02432 
02433     /* Write the number of profiles to request to the payload buffer */
02434     uint16_t temp_buffer = host_to_sick_ld_byte_order(num_profiles);
02435     memcpy(&payload_buffer[2],&temp_buffer,2);
02436 
02437     /* Set the profile format mask (for now, we request everything from the Sick LD) */
02438     temp_buffer = profile_format;
02439     temp_buffer = host_to_sick_ld_byte_order(temp_buffer);
02440     memcpy(&payload_buffer[4],&temp_buffer,2);
02441   
02442     /* Define the send message object */
02443     SickLDMessage send_message(payload_buffer,6);
02444     SickLDMessage recv_message;
02445   
02446     /* Send the request */
02447     if (num_profiles == 0) {
02448       std::cout << "\tRequesting " << _sickProfileFormatToString(profile_format) << " data stream from Sick LD..." << std::endl;
02449     } else {
02450       std::cout << "\tRequesting " << num_profiles << " " << _sickProfileFormatToString(profile_format) << " profiles from Sick LD..." << std::endl;
02451     }
02452 
02453     /* Request scan profiles from the Sick (empirically it can take the Sick up to a few seconds to respond) */
02454     try {
02455       _sendMessageAndGetReply(send_message,recv_message);
02456     }
02457        
02458     /* Handle a timeout! */
02459     catch (SickTimeoutException &sick_timeout_exception) {
02460       std::cerr << sick_timeout_exception.what() << std::endl;
02461       throw;
02462     }
02463     
02464     /* Handle I/O exceptions */
02465     catch (SickIOException &sick_io_exception) {
02466       std::cerr << sick_io_exception.what() << std::endl;
02467       throw;
02468     }
02469     
02470     /* Handle a returned error code */
02471     catch (SickErrorException &sick_error_exception) {
02472       std::cerr << sick_error_exception.what() << std::endl;
02473       throw;
02474     }
02475     
02476     /* A safety net */
02477     catch (...) {
02478       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
02479       throw;
02480     }  
02481   
02482     /* Reset the payload buffer */
02483     memset(payload_buffer,0,6);
02484 
02485     /* Acquire the payload of the response */
02486     recv_message.GetPayload(payload_buffer);
02487 
02488     /* Check to make sure the returned format is correct and there were no errors */
02489     memcpy(&temp_buffer,&payload_buffer[2],2);
02490     temp_buffer = sick_ld_to_host_byte_order(temp_buffer);
02491 
02492     /* Another sanity check */
02493     if (temp_buffer != profile_format) {
02494       throw SickErrorException("SickLD::_getSickScanProfiles: Incorrect profile format was returned by the Sick LD!");
02495     }
02496   
02497     /* Check if the data stream flags need to be set */
02498     if (num_profiles == 0 && profile_format == SICK_SCAN_PROFILE_RANGE) {
02499       _sick_streaming_range_data = true;
02500     }
02501     else if (num_profiles == 0 && profile_format == SICK_SCAN_PROFILE_RANGE_AND_ECHO) {
02502       _sick_streaming_range_and_echo_data = true;
02503     }
02504 
02505     /* Show some output */
02506     if (num_profiles == 0) {
02507       std::cout << "\t\tData stream started!" << std::endl;
02508     } else {
02509       std::cout << "\t\tSick LD sending " << num_profiles << " scan profiles!" << std::endl;
02510     }
02511   
02512     /* Success */
02513   }
02514 
02520   void SickLD::_parseScanProfile( uint8_t * const src_buffer, sick_ld_scan_profile_t &profile_data ) const {
02521 
02522     uint16_t profile_format = 0;
02523     unsigned int data_offset = 0;
02524 
02525     /* Extract the scan profile format from the buffer */
02526     memcpy(&profile_format,&src_buffer[data_offset],2);
02527     profile_format = sick_ld_to_host_byte_order(profile_format);
02528     data_offset += 2;
02529 
02530     /* Extract the number of sectors in the scan area */
02531     profile_data.num_sectors = src_buffer[data_offset+1];
02532     data_offset += 2;
02533 
02534     /* NOTE: For the following field definitions see page 32 of the
02535      *       Sick LD telegram listing.
02536      */
02537     uint16_t temp_buffer; // A temporary buffer
02538   
02539     /* Check if PROFILESENT is included */
02540     if (profile_format & 0x0001) {
02541       memcpy(&temp_buffer,&src_buffer[data_offset],2);
02542       profile_data.profile_number = sick_ld_to_host_byte_order(temp_buffer);
02543       data_offset += 2;
02544     }
02545   
02546     /* Check if PROFILECOUNT is included */
02547     if (profile_format & 0x0002) {
02548       memcpy(&temp_buffer,&src_buffer[data_offset],2);
02549       profile_data.profile_counter = sick_ld_to_host_byte_order(temp_buffer);
02550       data_offset += 2;
02551     }
02552   
02553     /* Check if LAYERNUM is included */
02554     if (profile_format & 0x0004) {
02555       memcpy(&temp_buffer,&src_buffer[data_offset],2);
02556       profile_data.layer_num = sick_ld_to_host_byte_order(temp_buffer);
02557       data_offset += 2;
02558     }
02559   
02560     /* The extraneous stuff is out of the way, now extract the data
02561      * for each of the sectors in the scan area...
02562      */
02563     for (unsigned int i=0; i < profile_data.num_sectors; i++) {
02564 
02565       /* Check if SECTORNUM is included */
02566       if (profile_format & 0x0008) {
02567         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02568         profile_data.sector_data[i].sector_num = sick_ld_to_host_byte_order(temp_buffer);
02569         data_offset += 2;
02570       }
02571       else {
02572         profile_data.sector_data[i].sector_num = 0;
02573       }
02574     
02575       /* Check if DIRSTEP is included */
02576       if (profile_format & 0x0010) {
02577         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02578         profile_data.sector_data[i].angle_step = ((double)sick_ld_to_host_byte_order(temp_buffer))/16;
02579         data_offset += 2;
02580       }
02581       else {
02582         profile_data.sector_data[i].angle_step = 0;
02583       }
02584     
02585       /* Check if POINTNUM is included */
02586       if (profile_format & 0x0020) {
02587         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02588         profile_data.sector_data[i].num_data_points = sick_ld_to_host_byte_order(temp_buffer);
02589         data_offset += 2;
02590       }
02591       else {
02592         profile_data.sector_data[i].num_data_points = 0;
02593       }
02594     
02595       /* Check if TSTART is included */
02596       if (profile_format & 0x0040) {
02597         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02598         profile_data.sector_data[i].timestamp_start = sick_ld_to_host_byte_order(temp_buffer);
02599         data_offset += 2;
02600       }
02601       else {
02602         profile_data.sector_data[i].timestamp_start = 0;
02603       }
02604       
02605       /* Check if STARTDIR is included */
02606       if (profile_format & 0x0080) {
02607         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02608         profile_data.sector_data[i].angle_start = ((double)sick_ld_to_host_byte_order(temp_buffer))/16;
02609         data_offset += 2;
02610       }
02611       else {
02612         profile_data.sector_data[i].angle_start = 0;
02613       }
02614     
02615       /* Acquire the range and echo values for the sector */
02616       for (unsigned int j=0; j < profile_data.sector_data[i].num_data_points; j++) {
02617 
02618         /* Check if DISTANCE-n is included */
02619         if (profile_format & 0x0100) {
02620           memcpy(&temp_buffer,&src_buffer[data_offset],2);
02621           profile_data.sector_data[i].range_values[j] = ((double)sick_ld_to_host_byte_order(temp_buffer))/256;
02622           data_offset += 2;
02623         }
02624         else {
02625           profile_data.sector_data[i].range_values[j] = 0;
02626         }
02627       
02628         /* Check if DIRECTION-n is included */
02629         if (profile_format & 0x0200) {
02630           memcpy(&temp_buffer,&src_buffer[data_offset],2);
02631           profile_data.sector_data[i].scan_angles[j] = ((double)sick_ld_to_host_byte_order(temp_buffer))/16;
02632           data_offset += 2;
02633         }
02634         else {
02635           profile_data.sector_data[i].scan_angles[j] = 0;
02636         }
02637       
02638         /* Check if ECHO-n is included */
02639         if (profile_format & 0x0400) {
02640           memcpy(&temp_buffer,&src_buffer[data_offset],2);
02641           profile_data.sector_data[i].echo_values[j] = sick_ld_to_host_byte_order(temp_buffer);
02642           data_offset += 2;      
02643         }
02644         else {
02645           profile_data.sector_data[i].echo_values[j] = 0;
02646         }
02647               
02648       }
02649 
02650       /* Check if TEND is included */
02651       if (profile_format & 0x0800) {
02652         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02653         profile_data.sector_data[i].timestamp_stop = sick_ld_to_host_byte_order(temp_buffer);
02654         data_offset += 2;
02655       }
02656       else {
02657         profile_data.sector_data[i].timestamp_stop = 0;
02658       }
02659     
02660       /* Check if ENDDIR is included */
02661       if (profile_format & 0x1000) {
02662         memcpy(&temp_buffer,&src_buffer[data_offset],2);
02663         profile_data.sector_data[i].angle_stop = ((double)sick_ld_to_host_byte_order(temp_buffer))/16;
02664         data_offset += 2;   
02665       }
02666       else {
02667         profile_data.sector_data[i].angle_stop = 0;
02668       }
02669     
02670     }
02671 
02672     /* Check if SENSTAT is included */
02673     if (profile_format & 0x2000) {
02674       profile_data.sensor_status = src_buffer[data_offset+3] & 0x0F;
02675       profile_data.motor_status = (src_buffer[data_offset+3] >> 4) & 0x0F;
02676     }
02677     else {
02678       profile_data.sensor_status = SICK_SENSOR_MODE_UNKNOWN;
02679       profile_data.motor_status = SICK_MOTOR_MODE_UNKNOWN;
02680     }
02681   
02682   }
02683 
02687   void SickLD::_cancelSickScanProfiles( ) 
02688     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02689 
02690     /* Ensure the device is in measurement mode */
02691     try {
02692       _setSickSensorModeToMeasure();
02693     }
02694        
02695     /* Handle a timeout! */
02696     catch (SickTimeoutException &sick_timeout_exception) {
02697       std::cerr << sick_timeout_exception.what() << std::endl;
02698       throw;
02699     }
02700     
02701     /* Handle I/O exceptions */
02702     catch (SickIOException &sick_io_exception) {
02703       std::cerr << sick_io_exception.what() << std::endl;
02704       throw;
02705     }
02706     
02707     /* Handle a returned error code */
02708     catch (SickErrorException &sick_error_exception) {
02709       std::cerr << sick_error_exception.what() << std::endl;
02710       throw;
02711     }
02712     
02713     /* A safety net */
02714     catch (...) {
02715       std::cerr << "SickLMS::_cancelSickScanProfiles: Unknown exception!!!" << std::endl;
02716       throw;
02717     }  
02718   
02719     /* Allocate a single buffer for payload contents */
02720     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02721 
02722     /* Set the service IDs */
02723     payload_buffer[0] = SICK_MEAS_SERV_CODE;           // Requested service type
02724     payload_buffer[1] = SICK_MEAS_SERV_CANCEL_PROFILE; // Requested service subtype
02725   
02726     /* Create the Sick messages */
02727     SickLDMessage send_message(payload_buffer,2);
02728     SickLDMessage recv_message;
02729 
02730     std::cout << "\tStopping the data stream..." << std::endl;
02731 
02732     /* Send the message and check the reply */
02733     try {
02734       _sendMessageAndGetReply(send_message,recv_message);
02735     }
02736     
02737     /* Handle a timeout! */
02738     catch (SickTimeoutException &sick_timeout_exception) {
02739       std::cerr << sick_timeout_exception.what() << std::endl;
02740       throw;
02741     }
02742     
02743     /* Handle I/O exceptions */
02744     catch (SickIOException &sick_io_exception) {
02745       std::cerr << sick_io_exception.what() << std::endl;
02746       throw;
02747     }
02748     
02749     /* A safety net */
02750     catch (...) {
02751       std::cerr << "SickLMS::_getFirmwareName: Unknown exception!!!" << std::endl;
02752       throw;
02753     }
02754   
02755     /* Reset the payload buffer */
02756     memset(payload_buffer,0,2);
02757   
02758     /* Update the status of ths Sick LD */
02759     recv_message.GetPayload(payload_buffer);
02760 
02761     /* Extract and assign the sensor and motor status */
02762     _sick_sensor_mode = payload_buffer[5] & 0x0F;
02763     _sick_motor_mode = (payload_buffer[5] >> 4) & 0x0F;
02764 
02765     /* Since we just updated them, let's make sure everything s'ok */
02766     if (_sick_sensor_mode == SICK_SENSOR_MODE_ERROR) {
02767       throw SickErrorException("SickLD::_cancelSickScanProfiles: Sick LD returned sensor mode ERROR!");
02768     }
02769 
02770     /* Check the motor mode */
02771     if (_sick_motor_mode == SICK_MOTOR_MODE_ERROR) {
02772       throw SickErrorException("SickLD::_cancelSickScanProfiles: Sick LD returned motor mode ERROR!");
02773     }
02774 
02775     /* Set the stream flag for the driver */
02776     if (_sick_streaming_range_data) {
02777       _sick_streaming_range_data = false;
02778     }
02779     else {
02780       _sick_streaming_range_and_echo_data = false;
02781     }
02782   
02783     std::cout << "\t\tStream stopped!" << std::endl;    
02784   }  
02785 
02790   void SickLD::_setSickFilter( const uint8_t suppress_code ) 
02791     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02792 
02793     /* Ensure the device is not measuring */
02794     if (_sick_sensor_mode == SICK_SENSOR_MODE_MEASURE) {
02795 
02796       /* Set the Sick LD to rotate mode */
02797       try {
02798         _setSickSensorModeToRotate();
02799       }
02800          
02801       /* Handle a timeout! */
02802       catch (SickTimeoutException &sick_timeout_exception) {
02803         std::cerr << sick_timeout_exception.what() << std::endl;
02804         throw;
02805       }
02806       
02807       /* Handle I/O exceptions */
02808       catch (SickIOException &sick_io_exception) {
02809         std::cerr << sick_io_exception.what() << std::endl;
02810         throw;
02811       }
02812       
02813       /* Handle a returned error code */
02814       catch (SickErrorException &sick_error_exception) {
02815         std::cerr << sick_error_exception.what() << std::endl;
02816         throw;
02817       }
02818       
02819       /* A safety net */
02820       catch (...) {
02821         std::cerr << "SickLMS::_setSickFilter: Unknown exception!!!" << std::endl;
02822         throw;
02823       }  
02824 
02825     }
02826   
02827     /* Allocate a single buffer for payload contents */
02828     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02829 
02830     /* Set the service IDs */
02831     payload_buffer[0] = SICK_CONF_SERV_CODE;                 // Requested service type
02832     payload_buffer[1] = SICK_CONF_SERV_SET_FILTER;           // Requested service subtype
02833     payload_buffer[3] = SICK_CONF_SERV_SET_FILTER_NEARFIELD; // Setting nearfield suppression filter    
02834     payload_buffer[5] = suppress_code;                       // Code telling whether to turn it on or off
02835   
02836     /* Create the Sick messages */
02837     SickLDMessage send_message(payload_buffer,6);
02838     SickLDMessage recv_message;
02839 
02840     /* Send the message and check the reply */
02841     try {
02842       _sendMessageAndGetReply(send_message,recv_message);
02843     }
02844        
02845     /* Handle a timeout! */
02846     catch (SickTimeoutException &sick_timeout_exception) {
02847       std::cerr << sick_timeout_exception.what() << std::endl;
02848       throw;
02849     }
02850     
02851     /* Handle I/O exceptions */
02852     catch (SickIOException &sick_io_exception) {
02853       std::cerr << sick_io_exception.what() << std::endl;
02854       throw;
02855     }
02856     
02857     /* A safety net */
02858     catch (...) {
02859       std::cerr << "SickLMS::_setSickFilter: Unknown exception!!!" << std::endl;
02860       throw;
02861     }
02862 
02863     /* Reset the payload buffer */
02864     memset(payload_buffer,0,6);
02865   
02866     /* Acquire the returned payload */
02867     recv_message.GetPayload(payload_buffer);
02868 
02869     /* Extract FILTERITEM */
02870     uint16_t filter_item;
02871     memcpy(&filter_item,&payload_buffer[2],2);
02872     filter_item = sick_ld_to_host_byte_order(filter_item);
02873 
02874     /* Check that the returned filter item matches nearfiled suppression */
02875     if (filter_item != SICK_CONF_SERV_SET_FILTER_NEARFIELD) {
02876       throw SickErrorException("SickLD::_setSickFilter: Unexpected filter item returned from Sick LD!");
02877     }
02878   
02879     /* Success */
02880   }
02881 
02885   void SickLD::_getSickIdentity( )  throw( SickTimeoutException, SickIOException ) {
02886 
02887     try {
02888       
02889       _getSensorPartNumber();
02890       _getSensorName();
02891       _getSensorVersion();
02892       _getSensorSerialNumber();
02893       _getSensorEDMSerialNumber();
02894       _getFirmwarePartNumber();
02895       _getFirmwareName();
02896       _getFirmwareVersion();
02897       _getApplicationSoftwarePartNumber();
02898       _getApplicationSoftwareName();
02899       _getApplicationSoftwareVersion();
02900 
02901     }
02902  
02903     /* Handle a timeout! */
02904     catch (SickTimeoutException &sick_timeout_exception) {
02905       std::cerr << sick_timeout_exception.what() << std::endl;
02906       throw;
02907     }
02908     
02909     /* Handle I/O exceptions */
02910     catch (SickIOException &sick_io_exception) {
02911       std::cerr << sick_io_exception.what() << std::endl;
02912       throw;
02913     }
02914     
02915     /* A safety net */
02916     catch (...) {
02917       std::cerr << "SickLMS::_getSickIdentity: Unknown exception!!!" << std::endl;
02918       throw;
02919     }
02920   
02921     /* Success! */
02922   }
02923 
02927   void SickLD::_getSickStatus( ) throw( SickTimeoutException, SickIOException ) {
02928 
02929     /* Allocate a single buffer for payload contents */
02930     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
02931 
02932     /* Set the service IDs */
02933     payload_buffer[0] = SICK_STAT_SERV_CODE;       // Requested service type
02934     payload_buffer[1] = SICK_STAT_SERV_GET_STATUS; // Requested service subtype
02935   
02936     /* Create the Sick messages */
02937     SickLDMessage send_message(payload_buffer,2);
02938     SickLDMessage recv_message;
02939   
02940     /* Send the message and check the reply */
02941     try {
02942       _sendMessageAndGetReply(send_message,recv_message);
02943     }
02944     
02945     catch(SickTimeoutException &sick_timeout_exception) {
02946       std::cerr << sick_timeout_exception.what() << std::endl;
02947       throw;
02948     }
02949 
02950     catch(SickIOException &sick_io_exception) {
02951       std::cerr << sick_io_exception.what() << std::endl;
02952       throw;
02953     }
02954 
02955     catch(...) {
02956       std::cerr << "SickLD::_getSickStatus - Unknown exception!" << std::endl;
02957       throw;
02958     }
02959     
02960     /* Reset the buffer (not necessary, but its better to do so just in case) */
02961     memset(payload_buffer,0,2);
02962   
02963     /* Extract the message payload */
02964     recv_message.GetPayload(payload_buffer);
02965 
02966     /* Extract the Sick LD's current sensor mode */
02967     _sick_sensor_mode = payload_buffer[5] & 0x0F;
02968 
02969     /* Extract the Sick LD's current motor mode */
02970     _sick_motor_mode = (payload_buffer[5] >> 4) & 0x0F;
02971 
02972     /* Success */
02973   }
02974 
02988   void SickLD::_setSickGlobalConfig( const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step )
02989     throw( SickErrorException, SickTimeoutException, SickIOException ) {
02990 
02991     /* Ensure the device is in IDLE mode */
02992     try {
02993       _setSickSensorModeToIdle();
02994     }
02995        
02996     /* Handle a timeout! */
02997     catch (SickTimeoutException &sick_timeout_exception) {
02998       std::cerr << sick_timeout_exception.what() << std::endl;
02999       throw;
03000     }
03001     
03002     /* Handle I/O exceptions */
03003     catch (SickIOException &sick_io_exception) {
03004       std::cerr << sick_io_exception.what() << std::endl;
03005       throw;
03006     }
03007     
03008     /* Handle a returned error code */
03009     catch (SickErrorException &sick_error_exception) {
03010       std::cerr << sick_error_exception.what() << std::endl;
03011       throw;
03012     }
03013     
03014     /* A safety net */
03015     catch (...) {
03016       std::cerr << "SickLMS::_setSickGlobalConfig: Unknown exception!!!" << std::endl;
03017       throw;
03018     }  
03019 
03020     /* Allocate a single buffer for payload contents */
03021     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
03022 
03023     /* Set the service IDs */
03024     payload_buffer[0] = SICK_CONF_SERV_CODE;                      // Requested service type
03025     payload_buffer[1] = SICK_CONF_SERV_SET_CONFIGURATION;         // Requested service subtype
03026 
03027     /* Set the configuration key */
03028     payload_buffer[3] = SICK_CONF_KEY_GLOBAL;                     // Use the global configuration key
03029 
03030     /* Set the message parameters */
03031     payload_buffer[5] = sick_sensor_id;                           // Include the given sensor ID
03032     payload_buffer[7] = sick_motor_speed;                         // Include the new Sick Motor speed value
03033 
03034     /* Set the angular step */
03035     uint16_t temp_buffer = _angleToTicks(sick_angle_step);
03036     temp_buffer = host_to_sick_ld_byte_order(temp_buffer);
03037     memcpy(&payload_buffer[8],&temp_buffer,2);
03038 
03039     /* Create the Sick messages */
03040     SickLDMessage send_message(payload_buffer,10);
03041     SickLDMessage recv_message;
03042 
03043     /* Send the message and check the reply */
03044     try {
03045       _sendMessageAndGetReply(send_message,recv_message);
03046     }
03047               
03048     /* Handle a timeout! */
03049     catch (SickTimeoutException &sick_timeout_exception) {
03050       std::cerr << sick_timeout_exception.what() << std::endl;
03051       throw;
03052     }
03053     
03054     /* Handle I/O exceptions */
03055     catch (SickIOException &sick_io_exception) {
03056       std::cerr << sick_io_exception.what() << std::endl;
03057       throw;
03058     }
03059     
03060     /* A safety net */
03061     catch (...) {
03062       std::cerr << "SickLMS::_setSickGlobalConfig: Unknown exception!!!" << std::endl;
03063       throw;
03064     }  
03065 
03066     /* Reset the payload buffer */
03067     memset(payload_buffer,0,10);
03068 
03069     /* Extract the response payload */
03070     recv_message.GetPayload(payload_buffer);
03071 
03072     /* Check to make sure there wasn't an error */
03073     if (payload_buffer[2] != 0 || payload_buffer[3] != 0) {
03074       throw SickErrorException("SickLD::_setSickGlobalConfig: Configuration setting was NOT sucessful!");
03075     }
03076 
03077     /* Update the device driver with the new values */
03078     _sick_global_config.sick_sensor_id = sick_sensor_id;
03079     _sick_global_config.sick_motor_speed = sick_motor_speed;
03080     _sick_global_config.sick_angle_step = sick_angle_step;  
03081     
03082     /* Success! */
03083   }
03084 
03088   void SickLD::_getSickGlobalConfig( )  throw( SickErrorException, SickTimeoutException, SickIOException ) {
03089 
03090     /* Ensure the device is in IDLE mode */
03091     try {
03092       _setSickSensorModeToIdle();
03093     }
03094        
03095     /* Handle a timeout! */
03096     catch (SickTimeoutException &sick_timeout_exception) {
03097       std::cerr << sick_timeout_exception.what() << std::endl;
03098       throw;
03099     }
03100     
03101     /* Handle I/O exceptions */
03102     catch (SickIOException &sick_io_exception) {
03103       std::cerr << sick_io_exception.what() << std::endl;
03104       throw;
03105     }
03106     
03107     /* Handle a returned error code */
03108     catch (SickErrorException &sick_error_exception) {
03109       std::cerr << sick_error_exception.what() << std::endl;
03110       throw;
03111     }
03112     
03113     /* A safety net */
03114     catch (...) {
03115       std::cerr << "SickLMS::_getSickGlobalConfig: Unknown exception!!!" << std::endl;
03116       throw;
03117     }  
03118   
03119     /* Allocate a single buffer for payload contents */
03120     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
03121 
03122     /* Set the service IDs */
03123     payload_buffer[0] = SICK_CONF_SERV_CODE;              // Requested service type
03124     payload_buffer[1] = SICK_CONF_SERV_GET_CONFIGURATION; // Requested service subtype
03125     payload_buffer[3] = SICK_CONF_KEY_GLOBAL;             // Configuration key
03126   
03127     /* Create the Sick messages */
03128     SickLDMessage send_message(payload_buffer,4);
03129     SickLDMessage recv_message;
03130   
03131     /* Send the message and check the reply */
03132     try {
03133       _sendMessageAndGetReply(send_message,recv_message);
03134     }
03135             
03136     /* Handle a timeout! */
03137     catch (SickTimeoutException &sick_timeout_exception) {
03138       std::cerr << sick_timeout_exception.what() << std::endl;
03139       throw;
03140     }
03141     
03142     /* Handle I/O exceptions */
03143     catch (SickIOException &sick_io_exception) {
03144       std::cerr << sick_io_exception.what() << std::endl;
03145       throw;
03146     }
03147     
03148     /* A safety net */
03149     catch (...) {
03150       std::cerr << "SickLMS::_getSickGlobalConfig: Unknown exception!!!" << std::endl;
03151       throw;
03152     }
03153 
03154     /* Reset the buffer (not necessary, but its better to do so just in case) */
03155     memset(payload_buffer,0,4);
03156   
03157     /* Extract the message payload */
03158     recv_message.GetPayload(payload_buffer);
03159 
03160     /* Extract the configuration key */
03161     uint16_t temp_buffer = 0;
03162     unsigned int data_offset = 2;
03163     memcpy(&temp_buffer,&payload_buffer[data_offset],2);
03164     temp_buffer = sick_ld_to_host_byte_order(temp_buffer);
03165     data_offset += 2;
03166 
03167     /* A quick sanity check */
03168     if (temp_buffer != SICK_CONF_KEY_GLOBAL) {
03169       throw SickErrorException("SickLD::_getSickGlobalConfig: Unexpected message contents!");
03170     }
03171 
03172     /* Extract the global sensor ID */
03173     memcpy(&_sick_global_config.sick_sensor_id,&payload_buffer[data_offset],2);
03174     _sick_global_config.sick_sensor_id = sick_ld_to_host_byte_order(_sick_global_config.sick_sensor_id);
03175     data_offset += 2;
03176   
03177     /* Extract the nominal motor speed */
03178     memcpy(&_sick_global_config.sick_motor_speed,&payload_buffer[data_offset],2);
03179     _sick_global_config.sick_motor_speed = sick_ld_to_host_byte_order(_sick_global_config.sick_motor_speed);
03180     data_offset += 2;
03181 
03182     /* Extract the angular step */
03183     memcpy(&temp_buffer,&payload_buffer[data_offset],2);
03184     _sick_global_config.sick_angle_step = _ticksToAngle(sick_ld_to_host_byte_order(temp_buffer));
03185   
03186     /* Success */
03187   }
03188 
03192   void SickLD::_getSickEthernetConfig( )
03193     throw( SickErrorException, SickTimeoutException, SickIOException ) {
03194 
03195     /* Ensure the device is in IDLE mode */
03196     try {      
03197       _setSickSensorModeToIdle();
03198     }
03199         
03200     /* Handle a timeout! */
03201     catch (SickTimeoutException &sick_timeout_exception) {
03202       std::cerr << sick_timeout_exception.what() << std::endl;
03203       throw;
03204     }
03205     
03206     /* Handle I/O exceptions */
03207     catch (SickIOException &sick_io_exception) {
03208       std::cerr << sick_io_exception.what() << std::endl;
03209       throw;
03210     }
03211     
03212     /* Handle a returned error code */
03213     catch (SickErrorException &sick_error_exception) {
03214       std::cerr << sick_error_exception.what() << std::endl;
03215       throw;
03216     }
03217     
03218     /* A safety net */
03219     catch (...) {
03220       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
03221       throw;
03222     }  
03223     
03224     /* Allocate a single buffer for payload contents */
03225     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
03226     
03227     /* Set the service IDs */
03228     payload_buffer[0] = SICK_CONF_SERV_CODE;              // Requested service type
03229     payload_buffer[1] = SICK_CONF_SERV_GET_CONFIGURATION; // Requested service subtype
03230     payload_buffer[3] = SICK_CONF_KEY_ETHERNET;           // Configuration key
03231     
03232     /* Create the Sick messages */
03233     SickLDMessage send_message(payload_buffer,4);
03234     SickLDMessage recv_message;
03235     
03236     try {
03237       _sendMessageAndGetReply(send_message,recv_message);
03238     }
03239         
03240     /* Handle a timeout! */
03241     catch (SickTimeoutException &sick_timeout_exception) {
03242       std::cerr << sick_timeout_exception.what() << std::endl;
03243       throw;
03244     }
03245     
03246     /* Handle I/O exceptions */
03247     catch (SickIOException &sick_io_exception) {
03248       std::cerr << sick_io_exception.what() << std::endl;
03249       throw;
03250     }
03251     
03252     /* A safety net */
03253     catch (...) {
03254       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
03255       throw;
03256     }  
03257     
03258     /* Reset the buffer (not necessary, but its better to do so just in case) */
03259     memset(payload_buffer,0,4);
03260   
03261     /* Extract the message payload */
03262     recv_message.GetPayload(payload_buffer);
03263 
03264     /* Extract the configuration key */
03265     uint16_t temp_buffer = 0;
03266     unsigned int data_offset = 2;
03267     memcpy(&temp_buffer,&payload_buffer[data_offset],2);
03268     temp_buffer = sick_ld_to_host_byte_order(temp_buffer);
03269     data_offset += 2;
03270 
03271     /* A quick sanity check */
03272     if (temp_buffer != SICK_CONF_KEY_ETHERNET) {
03273       throw SickErrorException("SickLD::_getSickEthernetConfig: Unexpected message contents!");
03274     }
03275   
03276     /* Extract the IP address of the Sick LD */
03277     for(unsigned int i=0; i < 4; i++,data_offset+=2) {
03278       memcpy(&_sick_ethernet_config.sick_ip_address[i],&payload_buffer[data_offset],2);
03279       _sick_ethernet_config.sick_ip_address[i] = sick_ld_to_host_byte_order(_sick_ethernet_config.sick_ip_address[i]);
03280     }
03281 
03282     /* Extract the associated subnet mask */
03283     for(unsigned int i=0; i < 4; i++,data_offset+=2) {
03284       memcpy(&_sick_ethernet_config.sick_subnet_mask[i],&payload_buffer[data_offset],2);
03285       _sick_ethernet_config.sick_subnet_mask[i] = sick_ld_to_host_byte_order(_sick_ethernet_config.sick_subnet_mask[i]);
03286     }
03287 
03288     /* Extract the default gateway */
03289     for(unsigned int i=0; i < 4; i++,data_offset+=2) {
03290       memcpy(&_sick_ethernet_config.sick_gateway_ip_address[i],&payload_buffer[data_offset],2);
03291       _sick_ethernet_config.sick_gateway_ip_address[i] = sick_ld_to_host_byte_order(_sick_ethernet_config.sick_gateway_ip_address[i]);
03292     }
03293 
03294     /* Extract the sick node ID (NOTE: This value doesn't matter, but we buffer it anyways) */
03295     memcpy(&_sick_ethernet_config.sick_node_id,&payload_buffer[data_offset],2);
03296     _sick_ethernet_config.sick_node_id = sick_ld_to_host_byte_order(_sick_ethernet_config.sick_node_id);
03297     data_offset += 2;
03298 
03299     /* Extract the transparent TCP port (NOTE: The significance of this value is unclear as
03300      * it doesn't affect the actual TCP port number that the Sick server is operating at.
03301      * But, we buffer it anyways as it is included in the configuration.)
03302      */
03303     memcpy(&_sick_ethernet_config.sick_transparent_tcp_port,&payload_buffer[data_offset],2);
03304     _sick_ethernet_config.sick_transparent_tcp_port = sick_ld_to_host_byte_order(_sick_ethernet_config.sick_transparent_tcp_port);
03305     data_offset += 2;
03306 
03307     /* Success */
03308   }
03309 
03318   void SickLD::_getSickSectorConfig( )
03319     throw( SickErrorException, SickTimeoutException, SickIOException ) {
03320 
03321     /* Reset the sector config struct */
03322     memset(&_sick_sector_config,0,sizeof(sick_ld_config_sector_t));
03323     
03324     /* Get the configuration for all initialized sectors */
03325     for (unsigned int i = 0; i < SICK_MAX_NUM_SECTORS; i++) {
03326       
03327       /* Query the Sick for the function of the ith sector */
03328       try {
03329         _getSickSectorFunction(i,_sick_sector_config.sick_sector_functions[i],_sick_sector_config.sick_sector_stop_angles[i]);
03330       }
03331       
03332       /* Handle a timeout! */
03333       catch (SickTimeoutException &sick_timeout_exception) {
03334         std::cerr << sick_timeout_exception.what() << std::endl;
03335         throw;
03336       }
03337       
03338       /* Handle I/O exceptions */
03339       catch (SickIOException &sick_io_exception) {
03340         std::cerr << sick_io_exception.what() << std::endl;
03341         throw;
03342       }
03343       
03344       /* Handle a returned error code */
03345       catch (SickErrorException &sick_error_exception) {
03346         std::cerr << sick_error_exception.what() << std::endl;
03347         throw;
03348       }
03349       
03350       /* A safety net */
03351       catch (...) {
03352         std::cerr << "SickLMS::_getSickSectorConfig: Unknown exception!!!" << std::endl;
03353         throw;
03354       } 
03355       
03356       /* Check if the sector is initialized */
03357       if (_sick_sector_config.sick_sector_functions[i] != SICK_CONF_SECTOR_NOT_INITIALIZED) {
03358         
03359         /* Check whether the sector is active (i.e. measuring) */
03360         if (_sick_sector_config.sick_sector_functions[i] == SICK_CONF_SECTOR_NORMAL_MEASUREMENT) {
03361           _sick_sector_config.sick_active_sector_ids[_sick_sector_config.sick_num_active_sectors] = i;
03362           _sick_sector_config.sick_num_active_sectors++;
03363         }
03364         
03365         /* Update the number of initialized sectors */
03366         _sick_sector_config.sick_num_initialized_sectors++;
03367       }    
03368       else {
03369         
03370         /* An uninitialized sector marks the end of the sector configuration */
03371         break;
03372       }
03373       
03374     } 
03375   
03376     /* Compute the starting angle for each of the initialized sectors */  
03377     for (unsigned int i = 1; i < _sick_sector_config.sick_num_initialized_sectors; i++) {
03378       _sick_sector_config.sick_sector_start_angles[i] = fmod(_sick_sector_config.sick_sector_stop_angles[i-1]+_sick_global_config.sick_angle_step,360);      
03379     }
03380 
03381     /* Determine the starting angle for the first sector */
03382     if (_sick_sector_config.sick_num_initialized_sectors > 1) {
03383       _sick_sector_config.sick_sector_start_angles[0] =
03384         fmod(_sick_sector_config.sick_sector_stop_angles[_sick_sector_config.sick_num_initialized_sectors-1]+_sick_global_config.sick_angle_step,360);
03385     }
03386   
03387     /* Success! */
03388   }
03389 
03395   void SickLD::_getIdentificationString( const uint8_t id_request_code, std::string &id_return_string )
03396     throw( SickTimeoutException, SickIOException ) {
03397     
03398     /* Allocate a single buffer for payload contents */
03399     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
03400 
03401     /* Set the service IDs */
03402     payload_buffer[0] = SICK_STAT_SERV_CODE;   // Requested service type
03403     payload_buffer[1] = SICK_STAT_SERV_GET_ID; // Requested service subtype                  
03404     payload_buffer[3] = id_request_code;       // ID information that is being requested
03405   
03406     /* Create the Sick LD messages */
03407     SickLDMessage send_message(payload_buffer,4);
03408     SickLDMessage recv_message;
03409 
03410     /* Send the message and get the reply */
03411     try {
03412       _sendMessageAndGetReply(send_message,recv_message);
03413     }
03414     
03415     /* Handle a timeout! */
03416     catch (SickTimeoutException &sick_timeout_exception) {
03417       std::cerr << sick_timeout_exception.what() << std::endl;
03418       throw;
03419     }
03420     
03421     /* Handle write buffer exceptions */
03422     catch (SickIOException &sick_io_exception) {
03423       std::cerr << sick_io_exception.what() << std::endl;
03424       throw;
03425     }
03426     
03427     /* A safety net */
03428     catch (...) {
03429       std::cerr << "SickLMS::_getIdentificationString: Unknown exception!!!" << std::endl;
03430       throw;
03431     }
03432 
03433     /* Reset the payload buffer (not necessary, but it doesn't hurt to be careful) */
03434     memset(payload_buffer,0,4);
03435   
03436     /* Extract the message payload */
03437     recv_message.GetPayload(payload_buffer);
03438 
03439     /* Assign the string */
03440     id_return_string = (char *) &payload_buffer[2];
03441 
03442     /* Success, woohooo! */
03443   }
03444 
03448   void SickLD::_getSensorPartNumber( )  throw( SickTimeoutException, SickIOException ) {
03449 
03450     /* Query the Sick LD */
03451     try {
03452       _getIdentificationString(SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM,_sick_identity.sick_part_number);
03453     }
03454           
03455     /* Handle a timeout! */
03456     catch (SickTimeoutException &sick_timeout_exception) {
03457       std::cerr << sick_timeout_exception.what() << std::endl;
03458       throw;
03459     }
03460     
03461     /* Handle I/O exceptions */
03462     catch (SickIOException &sick_io_exception) {
03463       std::cerr << sick_io_exception.what() << std::endl;
03464       throw;
03465     }
03466     
03467     /* A safety net */
03468     catch (...) {
03469       std::cerr << "SickLMS::_getSensorPartNumber: Unknown exception!!!" << std::endl;
03470       throw;
03471     }
03472 
03473     /* Success, woohooo! */
03474   }
03475 
03479   void SickLD::_getSensorName( )  throw( SickTimeoutException, SickIOException ) {
03480 
03481     /* Query the Sick LD */
03482     try {
03483       _getIdentificationString(SICK_STAT_SERV_GET_ID_SENSOR_NAME,_sick_identity.sick_name);
03484     }
03485 
03486     /* Handle a timeout! */
03487     catch (SickTimeoutException &sick_timeout_exception) {
03488       std::cerr << sick_timeout_exception.what() << std::endl;
03489       throw;
03490     }
03491     
03492     /* Handle I/O exceptions */
03493     catch (SickIOException &sick_io_exception) {
03494       std::cerr << sick_io_exception.what() << std::endl;
03495       throw;
03496     }
03497     
03498     /* A safety net */
03499     catch (...) {
03500       std::cerr << "SickLMS::_getSensorName: Unknown exception!!!" << std::endl;
03501       throw;
03502     }
03503   
03504     /* Ok */
03505   }
03506 
03510   void SickLD::_getSensorVersion( ) throw( SickTimeoutException, SickIOException ) {
03511 
03512     /* Get the id info */
03513     try {
03514       _getIdentificationString(SICK_STAT_SERV_GET_ID_SENSOR_VERSION,_sick_identity.sick_version);
03515     }
03516 
03517     /* Handle a timeout! */
03518     catch (SickTimeoutException &sick_timeout_exception) {
03519       std::cerr << sick_timeout_exception.what() << std::endl;
03520       throw;
03521     }
03522     
03523     /* Handle I/O exceptions */
03524     catch (SickIOException &sick_io_exception) {
03525       std::cerr << sick_io_exception.what() << std::endl;
03526       throw;
03527     }
03528     
03529     /* A safety net */
03530     catch (...) {
03531       std::cerr << "SickLMS::_getSensorVersion: Unknown exception!!!" << std::endl;
03532       throw;
03533     }
03534   
03535     /* Ok */
03536   }
03537 
03541   void SickLD::_getSensorSerialNumber( )  throw( SickTimeoutException, SickIOException ) {
03542 
03543     try {
03544       _getIdentificationString(SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM,_sick_identity.sick_serial_number);
03545     }
03546     
03547     /* Handle a timeout! */
03548     catch (SickTimeoutException &sick_timeout_exception) {
03549       std::cerr << sick_timeout_exception.what() << std::endl;
03550       throw;
03551     }
03552     
03553     /* Handle I/O exceptions */
03554     catch (SickIOException &sick_io_exception) {
03555       std::cerr << sick_io_exception.what() << std::endl;
03556       throw;
03557     }
03558     
03559     /* A safety net */
03560     catch (...) {
03561       std::cerr << "SickLMS::_getSensorSerialNumber: Unknown exception!!!" << std::endl;
03562       throw;
03563     }
03564   
03565     /* Ok */
03566   }
03567 
03571   void SickLD::_getSensorEDMSerialNumber( )  throw( SickTimeoutException, SickIOException ) {
03572 
03573     try {
03574       _getIdentificationString(SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM,_sick_identity.sick_edm_serial_number);
03575     }
03576     
03577     /* Handle a timeout! */
03578     catch (SickTimeoutException &sick_timeout_exception) {
03579       std::cerr << sick_timeout_exception.what() << std::endl;
03580       throw;
03581     }
03582     
03583     /* Handle I/O exceptions */
03584     catch (SickIOException &sick_io_exception) {
03585       std::cerr << sick_io_exception.what() << std::endl;
03586       throw;
03587     }
03588     
03589     /* A safety net */
03590     catch (...) {
03591       std::cerr << "SickLMS::_getSensorEDMSerialNumber: Unknown exception!!!" << std::endl;
03592       throw;
03593     }
03594 
03595     /* Ok */
03596   }
03597 
03601   void SickLD::_getFirmwarePartNumber( )  throw( SickTimeoutException, SickIOException ) {
03602 
03603     try {
03604       _getIdentificationString(SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM,_sick_identity.sick_firmware_part_number);
03605     }  
03606     
03607     /* Handle a timeout! */
03608     catch (SickTimeoutException &sick_timeout_exception) {
03609       std::cerr << sick_timeout_exception.what() << std::endl;
03610       throw;
03611     }
03612     
03613     /* Handle I/O exceptions */
03614     catch (SickIOException &sick_io_exception) {
03615       std::cerr << sick_io_exception.what() << std::endl;
03616       throw;
03617     }
03618     
03619     /* A safety net */
03620     catch (...) {
03621       std::cerr << "SickLMS::_getFirmwarePartNumber: Unknown exception!!!" << std::endl;
03622       throw;
03623     }
03624   
03625     /* Ok */
03626   }
03627 
03631   void SickLD::_getFirmwareName( )  throw( SickTimeoutException, SickIOException ) {
03632 
03633     try {
03634       _getIdentificationString(SICK_STAT_SERV_GET_ID_FIRMWARE_NAME,_sick_identity.sick_firmware_name);
03635     }
03636         
03637     /* Handle a timeout! */
03638     catch (SickTimeoutException &sick_timeout_exception) {
03639       std::cerr << sick_timeout_exception.what() << std::endl;
03640       throw;
03641     }
03642     
03643     /* Handle I/O exceptions */
03644     catch (SickIOException &sick_io_exception) {
03645       std::cerr << sick_io_exception.what() << std::endl;
03646       throw;
03647     }
03648     
03649     /* A safety net */
03650     catch (...) {
03651       std::cerr << "SickLMS::_getFirmwareName: Unknown exception!!!" << std::endl;
03652       throw;
03653     }
03654   
03655     /* Ok */
03656   }
03657 
03661   void SickLD::_getFirmwareVersion( ) throw( SickTimeoutException, SickIOException ){
03662 
03663     try {
03664       _getIdentificationString(SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION,_sick_identity.sick_firmware_version);
03665     }
03666     
03667     /* Handle a timeout! */
03668     catch (SickTimeoutException &sick_timeout_exception) {
03669       std::cerr << sick_timeout_exception.what() << std::endl;
03670       throw;
03671     }
03672     
03673     /* Handle I/O exceptions */
03674     catch (SickIOException &sick_io_exception) {
03675       std::cerr << sick_io_exception.what() << std::endl;
03676       throw;
03677     }
03678     
03679     /* A safety net */
03680     catch (...) {
03681       std::cerr << "SickLMS::_getFirmwareVersion: Unknown exception!!!" << std::endl;
03682       throw;
03683     }
03684     
03685     /* Ok */
03686   }
03687 
03691   void SickLD::_getApplicationSoftwarePartNumber( )  throw( SickTimeoutException, SickIOException ) {
03692 
03693     try {
03694       _getIdentificationString(SICK_STAT_SERV_GET_ID_APP_PART_NUM,_sick_identity.sick_application_software_part_number);
03695     }
03696 
03697     /* Handle a timeout! */
03698     catch (SickTimeoutException &sick_timeout_exception) {
03699       std::cerr << sick_timeout_exception.what() << std::endl;
03700       throw;
03701     }
03702     
03703     /* Handle I/O exceptions */
03704     catch (SickIOException &sick_io_exception) {
03705       std::cerr << sick_io_exception.what() << std::endl;
03706       throw;
03707     }
03708     
03709     /* A safety net */
03710     catch (...) {
03711       std::cerr << "SickLMS::_getApplicationSoftwarePartNumber: Unknown exception!!!" << std::endl;
03712       throw;
03713     }
03714   
03715     /* Ok */
03716   }
03717 
03721   void SickLD::_getApplicationSoftwareName( )  throw( SickTimeoutException, SickIOException ) {
03722 
03723     try {
03724       _getIdentificationString(SICK_STAT_SERV_GET_ID_APP_NAME,_sick_identity.sick_application_software_name);
03725     }
03726     
03727     /* Handle a timeout! */
03728     catch (SickTimeoutException &sick_timeout_exception) {
03729       std::cerr << sick_timeout_exception.what() << std::endl;
03730       throw;
03731     }
03732     
03733     /* Handle I/O exceptions */
03734     catch (SickIOException &sick_io_exception) {
03735       std::cerr << sick_io_exception.what() << std::endl;
03736       throw;
03737     }
03738     
03739     /* A safety net */
03740     catch (...) {
03741       std::cerr << "SickLMS::_getApplication Software Name: Unknown exception!!!" << std::endl;
03742       throw;
03743     }
03744   
03745     /* Ok */
03746   }
03747 
03751   void SickLD::_getApplicationSoftwareVersion( )  throw( SickTimeoutException, SickIOException ) {
03752 
03753     try {
03754       _getIdentificationString(SICK_STAT_SERV_GET_ID_APP_VERSION,_sick_identity.sick_application_software_version);
03755     }
03756 
03757     /* Handle a timeout! */
03758     catch (SickTimeoutException &sick_timeout_exception) {
03759       std::cerr << sick_timeout_exception.what() << std::endl;
03760       throw;
03761     }
03762     
03763     /* Handle I/O exceptions */
03764     catch (SickIOException &sick_io_exception) {
03765       std::cerr << sick_io_exception.what() << std::endl;
03766       throw;
03767     }
03768     
03769     /* A safety net */
03770     catch (...) {
03771       std::cerr << "SickLMS::_getApplicationSoftwareVersion: Unknown exception!!!" << std::endl;
03772       throw;
03773     }
03774   
03775     /* Ok */
03776   }
03777 
03786   void SickLD::_setSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed, const double sick_angle_step,
03787                                                  const double * const active_sector_start_angles,
03788                                                  const double * const active_sector_stop_angles,
03789                                                  const unsigned int num_active_sectors )
03790     throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException ) {
03791 
03792     /* Define buffers to hold the device-ready configuration */
03793     unsigned int num_sectors = 0;
03794     unsigned int sector_functions[SICK_MAX_NUM_SECTORS] = {0};
03795     double sector_stop_angles[SICK_MAX_NUM_SECTORS] = {0};
03796   
03797     /* A few dummy buffers */
03798     double sorted_active_sector_start_angles[SICK_MAX_NUM_SECTORS] = {0};
03799     double sorted_active_sector_stop_angles[SICK_MAX_NUM_SECTORS] = {0};
03800   
03801     /* Begin by checking the num of active sectors */
03802     if (num_active_sectors > SICK_MAX_NUM_SECTORS/2) {
03803       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid number of active scan sectors!");
03804     }
03805 
03806     /* Ensure the given motor speed is valid (within proper bounds, etc...) */
03807     if (!_validSickMotorSpeed(sick_motor_speed)) {
03808       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid motor speed!");
03809     }
03810 
03811     /* Ensure the scan resolution is valid (within proper bounds, etc...) */
03812     if (!_validSickScanResolution(sick_angle_step,active_sector_start_angles,active_sector_stop_angles,num_active_sectors)) {
03813       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid scan resolution!");
03814     }
03815 
03816     /* Copy the input arguments */
03817     memcpy(sorted_active_sector_start_angles,active_sector_start_angles,sizeof(sorted_active_sector_start_angles));
03818     memcpy(sorted_active_sector_stop_angles,active_sector_stop_angles,sizeof(sorted_active_sector_stop_angles));
03819 
03820     /* Ensure a proper ordering of the given sector angle sets */
03821     _sortScanAreas(sorted_active_sector_start_angles,sorted_active_sector_stop_angles,num_active_sectors);
03822     
03823     /* Check for an invalid configuration */
03824     if (!_validActiveSectors(sorted_active_sector_start_angles,sorted_active_sector_stop_angles,num_active_sectors)) {
03825       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid sector configuration!");
03826     }
03827   
03828     /* Ensure the resulting pulse frequency is valid for the device */
03829     if (!_validPulseFrequency(sick_motor_speed,sick_angle_step,sorted_active_sector_start_angles, sorted_active_sector_stop_angles,num_active_sectors)) {
03830       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid pulse frequency!");
03831     }
03832 
03833     /* Generate the corresponding device-ready sector config */
03834     _generateSickSectorConfig(sorted_active_sector_start_angles,sorted_active_sector_stop_angles,num_active_sectors,sick_angle_step,
03835                               sector_functions,sector_stop_angles,num_sectors);
03836 
03837     try {
03838   
03839       /* Set the new sector configuration */
03840       _setSickSectorConfig(sector_functions,sector_stop_angles,num_sectors,false);
03841 
03842       /* Assign the new configuration in the flash
03843        *
03844        * NOTE: The following function must be called, even if the global parameters (motor speed,
03845        *       and angular resolution) are the same, in order to write the sector configuration.
03846        *       Why this is the case isn't exactly clear as the manual does not explain.
03847        */
03848       _setSickGlobalConfig(GetSickSensorID(),sick_motor_speed,sick_angle_step);
03849 
03850     }
03851     
03852     /* Handle a timeout! */
03853     catch (SickTimeoutException &sick_timeout_exception) {
03854       std::cerr << sick_timeout_exception.what() << std::endl;
03855       throw;
03856     }
03857     
03858     /* Handle I/O exceptions */
03859     catch (SickIOException &sick_io_exception) {
03860       std::cerr << sick_io_exception.what() << std::endl;
03861       throw;
03862     }
03863     
03864     /* Handle a returned error code */
03865     catch (SickErrorException &sick_error_exception) {
03866       std::cerr << sick_error_exception.what() << std::endl;
03867       throw;
03868     }
03869     
03870     /* A safety net */
03871     catch (...) {
03872       std::cerr << "SickLMS::_setSickGlobalParamsAndScanAreas: Unknown exception!!!" << std::endl;
03873       throw;
03874     }  
03875     
03876     /* Success! */
03877   }
03878 
03885   void SickLD::_setSickTemporaryScanAreas( const double * const active_sector_start_angles,
03886                                            const double * const active_sector_stop_angles,
03887                                            const unsigned int num_active_sectors )   throw( SickTimeoutException, SickIOException, SickConfigException ) {
03888 
03889     /* Define buffers to hold the device-ready configuration */
03890     unsigned int num_sectors = 0;
03891     unsigned int sector_functions[SICK_MAX_NUM_SECTORS] = {0};
03892     double sector_stop_angles[SICK_MAX_NUM_SECTORS] = {0};
03893   
03894     /* A few dummy buffers */
03895     double sorted_active_sector_start_angles[SICK_MAX_NUM_SECTORS] = {0};
03896     double sorted_active_sector_stop_angles[SICK_MAX_NUM_SECTORS] = {0};
03897   
03898     /* Begin by checking the num of active sectors */
03899     if (num_active_sectors > SICK_MAX_NUM_SECTORS/2)
03900       throw SickConfigException("_setSickTemporaryScanAreas: Invalid number of active scan sectors!");
03901 
03902     /* Copy the input arguments */
03903     memcpy(sorted_active_sector_start_angles,active_sector_start_angles,sizeof(sorted_active_sector_start_angles));
03904     memcpy(sorted_active_sector_stop_angles,active_sector_stop_angles,sizeof(sorted_active_sector_stop_angles));
03905 
03906     /* Ensure a proper ordering of the given sector angle sets */
03907     _sortScanAreas(sorted_active_sector_start_angles,sorted_active_sector_stop_angles,num_active_sectors);
03908 
03909     /* Check for an invalid configuration */
03910     if (!_validActiveSectors(sorted_active_sector_start_angles,sorted_active_sector_stop_angles,num_active_sectors)) {
03911       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid sector configuration!");
03912     }
03913     
03914     /* Ensure the resulting pulse frequency is valid for the device */
03915     if (!_validPulseFrequency(GetSickMotorSpeed(),GetSickScanResolution(),sorted_active_sector_start_angles,
03916                               sorted_active_sector_stop_angles,num_active_sectors)) {
03917       throw SickConfigException("SickLD::_setSickGlobalParamsAndScanAreas: Invalid pulse frequency!");
03918     }
03919     
03920     /* Generate the corresponding device-ready sector config */
03921     _generateSickSectorConfig(sorted_active_sector_start_angles,sorted_active_sector_stop_angles,num_active_sectors,GetSickScanResolution(),
03922                               sector_functions,sector_stop_angles,num_sectors);
03923 
03924     /* Set the new sector configuration */
03925     try {
03926       _setSickSectorConfig(sector_functions,sector_stop_angles,num_sectors);
03927     }
03928 
03929     /* Handle a timeout! */
03930     catch (SickTimeoutException &sick_timeout_exception) {
03931       std::cerr << sick_timeout_exception.what() << std::endl;
03932       throw;
03933     }
03934     
03935     /* Handle I/O exceptions */
03936     catch (SickIOException &sick_io_exception) {
03937       std::cerr << sick_io_exception.what() << std::endl;
03938       throw;
03939     }
03940     
03941     /* Handle a returned error code */
03942     catch (SickErrorException &sick_error_exception) {
03943       std::cerr << sick_error_exception.what() << std::endl;
03944       throw;
03945     }
03946     
03947     /* A safety net */
03948     catch (...) {
03949       std::cerr << "SickLMS::_setSickTemporaryScanAreas: Unknown exception!!!" << std::endl;
03950       throw;
03951     }  
03952     
03953     /* Success! */ 
03954   }
03955 
03963   void SickLD::_setSickSectorConfig( const unsigned int * const sector_functions, const double * const sector_stop_angles,
03964                                      const unsigned int num_sectors, const bool write_to_flash ) 
03965                                      throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) {
03966 
03967     /* Assign the new sector configuration to the device */
03968     for (unsigned int sector_id = 0; sector_id < num_sectors; sector_id++) {
03969 
03970       try {
03971         
03972         /* Set the corresponding sector function */
03973         _setSickSectorFunction(sector_id,sector_functions[sector_id],sector_stop_angles[sector_id],write_to_flash);
03974     
03975         /* Resync the driver with the new sector configuration */
03976         _getSickSectorConfig();
03977 
03978       }
03979       
03980       /* Handle a timeout! */
03981       catch (SickTimeoutException &sick_timeout_exception) {
03982         std::cerr << sick_timeout_exception.what() << std::endl;
03983         throw;
03984       }
03985       
03986       /* Handle I/O exceptions */
03987       catch (SickIOException &sick_io_exception) {
03988         std::cerr << sick_io_exception.what() << std::endl;
03989         throw;
03990       }
03991       
03992       /* Handle a returned error code */
03993       catch (SickErrorException &sick_error_exception) {
03994         std::cerr << sick_error_exception.what() << std::endl;
03995         throw;
03996       }
03997       
03998       /* Handle a returned error code */
03999       catch (SickConfigException &sick_config_exception) {
04000         std::cerr << sick_config_exception.what() << std::endl;
04001         throw;
04002       }
04003       
04004       /* A safety net */
04005       catch (...) {
04006         std::cerr << "SickLMS::_setSickSectorConfig: Unknown exception!!!" << std::endl;
04007         throw;
04008       }  
04009 
04010     }
04011   
04012     /* Success */
04013   }
04014 
04023   void SickLD::_setSickSignals( const uint8_t sick_signal_flags )  throw( SickIOException, SickTimeoutException, SickErrorException ) {
04024 
04025     /* Allocate a single buffer for payload contents */
04026     uint8_t payload_buffer[SickLDMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
04027 
04028     /* Set the service IDs */
04029     payload_buffer[0] = SICK_STAT_SERV_CODE;       // Requested service type
04030     payload_buffer[1] = SICK_STAT_SERV_SET_SIGNAL; // Requested service subtype
04031     payload_buffer[3] = sick_signal_flags;         // PORTVAL
04032   
04033     /* Create the Sick message */
04034     SickLDMessage send_message(payload_buffer,4);
04035     SickLDMessage recv_message;
04036   
04037     /* Send the message and get a response */
04038     try {
04039       _sendMessageAndGetReply(send_message,recv_message);
04040     }
04041            
04042     /* Handle a timeout! */
04043     catch (SickTimeoutException &sick_timeout_exception) {
04044       std::cerr << sick_timeout_exception.what() << std::endl;
04045       throw;
04046     }
04047     
04048     /* Handle I/O exceptions */
04049     catch (SickIOException &sick_io_exception) {
04050       std::cerr << sick_io_exception.what() << std::endl;
04051       throw;
04052     }
04053     
04054     /* A safety net */
04055     catch (...) {
04056       std::cerr << "SickLMS::_setSickSensorMode: Unknown exception!!!" << std::endl;
04057       throw;
04058     }
04059 
04060     /* Reset the payload buffer */
04061     memset(payload_buffer,0,4);
04062   
04063     /* Extract the message payload */
04064     recv_message.GetPayload(payload_buffer);
04065 
04066     /* Check to see if there was an error */
04067     if (payload_buffer[2] != 0) {
04068       throw SickErrorException("SickLD::_setSickSignals: Command failed!");
04069     }
04070   
04071     /* Success */
04072   }
04073 
04083   void SickLD::_sendMessageAndGetReply( const SickLDMessage &send_message, 
04084                                         SickLDMessage &recv_message, 
04085                                         const unsigned int timeout_value ) throw( SickIOException, SickTimeoutException ) {
04086 
04087     uint8_t byte_sequence[2] = {0};
04088 
04089     byte_sequence[0] = send_message.GetServiceCode() | 0x80;
04090     byte_sequence[1] = send_message.GetServiceSubcode();
04091 
04092     /* Send message and get reply using parent's method */
04093     try {
04094       SickLIDAR< SickLDBufferMonitor, SickLDMessage >::_sendMessageAndGetReply(send_message,recv_message,byte_sequence,2,0,DEFAULT_SICK_MESSAGE_TIMEOUT,1);
04095     }
04096         
04097     /* Handle a timeout! */
04098     catch (SickTimeoutException &sick_timeout_exception) {
04099       std::cerr << sick_timeout_exception.what() << std::endl;
04100       throw;
04101     }
04102     
04103     /* Handle write buffer exceptions */
04104     catch (SickIOException &sick_io_exception) {
04105       std::cerr << sick_io_exception.what() << std::endl;
04106       throw;
04107     }
04108     
04109     /* A safety net */
04110     catch (...) {
04111       std::cerr << "SickLD::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
04112       throw;
04113     }
04114 
04115   }
04116 
04120   void SickLD::_flushTCPRecvBuffer( ) throw( SickIOException, SickThreadException ) {
04121 
04122     uint8_t null_byte;
04123     int num_bytes_waiting = 0;    
04124 
04125     try {
04126     
04127       /* Acquire access to the data stream */
04128       _sick_buffer_monitor->AcquireDataStream();
04129       
04130       /* Acquire the number of the bytes awaiting read */
04131       if (ioctl(_sick_fd,FIONREAD,&num_bytes_waiting)) {
04132         throw SickIOException("SickLD::_flushTCPRecvBuffer: ioctl() failed! (Couldn't get the number of bytes awaiting read!)");
04133       }
04134       
04135       /* Read off the bytes awaiting in the buffer */
04136       for (int i = 0; i < num_bytes_waiting; i++) {
04137         read(_sick_fd,&null_byte,1);
04138       }
04139       
04140       /* Release the stream */
04141       _sick_buffer_monitor->ReleaseDataStream();
04142 
04143     }
04144 
04145     /* Catch any serious IO exceptions */
04146     catch(SickIOException &sick_io_exception) {
04147       std::cerr << sick_io_exception.what() << std::endl;
04148       throw;
04149     }
04150     
04151     /* Handle thread exceptions */
04152     catch(SickThreadException &sick_thread_exception) {
04153       std::cerr << sick_thread_exception.what() << std::endl;
04154       throw;
04155     }
04156 
04157     /* A sanity check */
04158     catch(...) {
04159       std::cerr << "SickLMS::_flushTerminalBuffer: Unknown exception!" << std::endl;
04160       throw;
04161     }
04162     
04163   }
04164   
04168   void SickLD::_teardownConnection( ) throw( SickIOException ) {
04169 
04170     /* Close the socket! */
04171     if (close(_sick_fd) < 0) {
04172       throw SickIOException("SickLD::_teardownConnection: close() failed!");
04173     }
04174   
04175   }
04176 
04187   void SickLD::_generateSickSectorConfig( const double * const active_sector_start_angles,
04188                                           const double * const active_sector_stop_angles,
04189                                           const unsigned int num_active_sectors,
04190                                           const double sick_angle_step,
04191                                           unsigned int * const sector_functions,
04192                                           double * const sector_stop_angles,
04193                                           unsigned int &num_sectors ) const {
04194 
04195     num_sectors = 0;
04196 
04197     /* Generate the sector configuration for multiple sectors */
04198     double final_diff = 0;
04199     if (num_active_sectors > 1) {
04200     
04201       /* Generate the actual sector configuration for the device */
04202       for (unsigned int i = 0; i < num_active_sectors; i++) {
04203 
04204         /* Insert the measurement sector for the active area */
04205         sector_functions[num_sectors] = SICK_CONF_SECTOR_NORMAL_MEASUREMENT;
04206         sector_stop_angles[num_sectors] = active_sector_stop_angles[i];
04207         num_sectors++;  
04208       
04209         /* Check whether to insert a non-measurement sector */
04210         if ((i < num_active_sectors - 1) && (active_sector_start_angles[i+1]-active_sector_stop_angles[i] >= 2*sick_angle_step)) {
04211 
04212           /* Set the next sector function as non-measurement */
04213           sector_functions[num_sectors] = SICK_CONF_SECTOR_NO_MEASUREMENT;
04214           sector_stop_angles[num_sectors] = active_sector_start_angles[i+1] - sick_angle_step;
04215           num_sectors++;                
04216 
04217         }
04218       
04219       }
04220 
04221       /* Compute the difference between the final stop angle and the first start angle*/
04222       if (active_sector_stop_angles[num_active_sectors-1] < active_sector_start_angles[0]) {
04223         final_diff = active_sector_start_angles[0] - active_sector_stop_angles[num_active_sectors-1];
04224       }
04225       else {
04226         final_diff = active_sector_start_angles[0] + (360 - active_sector_stop_angles[num_active_sectors-1]);
04227       }        
04228     
04229     }
04230     else {
04231     
04232       /* Insert the measurement sector for the active area */
04233       sector_functions[num_sectors] = SICK_CONF_SECTOR_NORMAL_MEASUREMENT;
04234       sector_stop_angles[num_sectors] = active_sector_stop_angles[0];
04235       num_sectors++;    
04236     
04237       /* Compute the difference between the final stop angle and the first start angle*/
04238       if (active_sector_stop_angles[0] <= active_sector_start_angles[0]) {
04239         final_diff = active_sector_start_angles[0] - active_sector_stop_angles[num_active_sectors-1];
04240       }
04241       else {
04242         final_diff = active_sector_start_angles[0] + (360 - active_sector_stop_angles[num_active_sectors-1]);
04243       }        
04244     
04245     }
04246 
04247     /* Check whether to add a final non-measurement sector */
04248     if (final_diff >= 2*sick_angle_step) {
04249 
04250       /* Include the final non-measurement sector */
04251       sector_functions[num_sectors] = SICK_CONF_SECTOR_NO_MEASUREMENT;
04252       sector_stop_angles[num_sectors] = active_sector_start_angles[0]-sick_angle_step +
04253                                         360*(sick_angle_step > active_sector_start_angles[0]);
04254       num_sectors++;
04255     
04256     }    
04257 
04258     /* If necessary insert the non-initialized sector */
04259     if (num_sectors < SICK_MAX_NUM_SECTORS) {
04260 
04261       /* Include the uninitialized sector */
04262       sector_functions[num_sectors] = SICK_CONF_SECTOR_NOT_INITIALIZED;
04263       sector_stop_angles[num_sectors] = 0;
04264       num_sectors++;        
04265 
04266     }
04267 
04268   }
04269 
04275   double SickLD::_ticksToAngle( const uint16_t ticks ) const {
04276     return (((double)ticks)/16);
04277   }
04278 
04288   uint16_t SickLD::_angleToTicks( const double angle ) const {
04289     return (uint16_t)(angle*16);
04290   }
04291 
04299   double SickLD::_computeMeanPulseFrequency( const double active_scan_area, const double curr_motor_speed,
04300                                              const double curr_angular_resolution ) const {
04301     /* Compute the mean pulse frequency */
04302     return _computeMaxPulseFrequency(SICK_MAX_SCAN_AREA,curr_motor_speed,curr_angular_resolution)*(active_scan_area/((double)SICK_MAX_SCAN_AREA));
04303   }
04304 
04312   double SickLD::_computeMaxPulseFrequency( const double total_scan_area, const double curr_motor_speed,
04313                                             const double curr_angular_resolution ) const {
04314     /* Compute the maximum pulse frequency */
04315     return total_scan_area*curr_motor_speed*(1/curr_angular_resolution);
04316   }
04317 
04322   bool SickLD::_validSickSensorID( const unsigned int sick_sensor_id ) const {
04323     
04324     /* Ensure the sensor ID is valid */
04325     if (sick_sensor_id < SICK_MIN_VALID_SENSOR_ID || sick_sensor_id > SICK_MAX_VALID_SENSOR_ID) {
04326       return false;
04327     }  
04328     
04329     /* Success */
04330     return true;
04331   }
04332 
04337   bool SickLD::_validSickMotorSpeed( const unsigned int sick_motor_speed ) const {
04338 
04339     /* Check the validity of the new Sick LD motor speed */
04340     if (sick_motor_speed < SICK_MIN_MOTOR_SPEED || sick_motor_speed > SICK_MAX_MOTOR_SPEED) {
04341       return false;
04342     }
04343 
04344     /* Success */
04345     return true;
04346   }
04347 
04354   bool SickLD::_validSickScanResolution( const double sick_angle_step, const double * const sector_start_angles,
04355                                          const double * const sector_stop_angles, const unsigned int num_sectors ) const {
04356 
04357     /* Check the validity of the new Sick LD angular step */
04358     if (sick_angle_step < SICK_MAX_SCAN_ANGULAR_RESOLUTION || fmod(sick_angle_step,SICK_MAX_SCAN_ANGULAR_RESOLUTION) != 0) {
04359       std::cerr << "Invalid scan resolution! (should be a positive multiple of " << SICK_MAX_SCAN_ANGULAR_RESOLUTION << ")" << std::endl;
04360       return false;
04361     }
04362   
04363     /* Ensure that the sector boundaries are divisible by the desired step angle */
04364     for(unsigned int i = 0; i < num_sectors; i++) {
04365       
04366       /* Check both the sector start and stop angles */
04367       if (fmod(sector_start_angles[i],sick_angle_step) != 0 || fmod(sector_stop_angles[i],sick_angle_step) != 0) {
04368         std::cerr << "Invalid scan resolution! (sector boundaries must be evenly divisible by the step angle)" << std::endl;
04369         return false;
04370       }
04371     
04372     }
04373 
04374     /* Success */
04375     return true;
04376   }
04377 
04383   bool SickLD::_validPulseFrequency( const unsigned int sick_motor_speed, const double sick_angle_step ) const {
04384 
04385     /* Simply call the other function w/ the current sector config and the given motor and step angle values */
04386     if(!_validPulseFrequency(sick_motor_speed,sick_angle_step,_sick_sector_config.sick_sector_start_angles,
04387                              _sick_sector_config.sick_sector_stop_angles,_sick_sector_config.sick_num_active_sectors)) {
04388       return false;
04389     }
04390     
04391     /* Valid! */
04392     return true;
04393   }
04394 
04403   bool SickLD::_validPulseFrequency( const unsigned int sick_motor_speed, const double sick_angle_step,
04404                                      const double * const active_sector_start_angles,
04405                                      const double * const active_sector_stop_angles,
04406                                      const unsigned int num_active_sectors ) const {
04407   
04408     /* Compute the scan area */
04409     double scan_area = _computeScanArea(sick_angle_step,active_sector_start_angles,active_sector_stop_angles,num_active_sectors);
04410   
04411     /* Check the mean pulse rate of the desired configuration */
04412     if (_computeMeanPulseFrequency(scan_area,sick_motor_speed,sick_angle_step) > SICK_MAX_MEAN_PULSE_FREQUENCY) { 
04413       std::cerr << "Max mean pulse frequency exceeded! (try a slower motor speed, a larger step angle and/or a smaller active scan area)" << std::endl;
04414       return false;
04415     }
04416 
04417     /* Check the maximum pulse rate of the desired configuration */
04418     if (_computeMaxPulseFrequency(SICK_MAX_SCAN_AREA,sick_motor_speed,sick_angle_step) > SICK_MAX_PULSE_FREQUENCY) { 
04419       std::cerr << "Max pulse frequency exceeded! (try a slower motor speed, a larger step angle and/or a smaller active scan area)" << std::endl;
04420       return false;
04421     }
04422   
04423     /* Valid! */
04424     return true;
04425   }
04426 
04444   double SickLD::_computeScanArea( const double sick_angle_step, const double * const active_sector_start_angles,
04445                                    const double * const active_sector_stop_angles, const unsigned int num_active_sectors ) const {
04446 
04447     /* Define the current scan area */
04448     double total_scan_area = 0;
04449     double curr_sector_scan_area = 0;
04450   
04451     /* For each sector given sum the absolute scan area for it */
04452     for (unsigned int i = 0; i < num_active_sectors; i++) {
04453 
04454       /* Compute the total scan area for this sector */
04455       curr_sector_scan_area = fabs(active_sector_start_angles[i]-active_sector_stop_angles[i]);
04456 
04457       /* Update the total scan area */
04458       total_scan_area += curr_sector_scan_area + sick_angle_step;
04459     }
04460   
04461     /* Return the computed area */
04462     return total_scan_area;  
04463 
04464   }
04465 
04472   void SickLD::_sortScanAreas( double * const sector_start_angles, double * const sector_stop_angles,
04473                                const unsigned int num_sectors ) const {
04474 
04475     /* A dummy temp variable */
04476     double temp = 0;
04477   
04478     /* Employ a simple bubblesort (NOTE: Only at most a handful of values will have to be sorted) */
04479     for (unsigned int i = 0; i < num_sectors; i++) {
04480       for (unsigned int j = (num_sectors-1); j > i; j--) {
04481         if (sector_start_angles[j] < sector_start_angles[j-1]) {
04482           SWAP_VALUES(sector_start_angles[j],sector_start_angles[j-1],temp);
04483           SWAP_VALUES(sector_stop_angles[j],sector_stop_angles[j-1],temp);
04484         }
04485       }
04486     }
04487   
04488   }
04489 
04496   bool SickLD::_validActiveSectors( const double * const sector_start_angles, const double * const sector_stop_angles,
04497                                     const unsigned int num_sectors ) const {
04498 
04499     /* A sanity check to make sure all are in [0,360) */
04500     for (unsigned int i = 0; i < num_sectors; i++) {
04501 
04502       if (sector_start_angles[i] < 0 || sector_stop_angles[i] < 0 ||
04503           sector_start_angles[i] >= 360 || sector_stop_angles[i] >= 360) {
04504 
04505         std::cerr << "Invalid sector config! (all degree values must be in [0,360))" << std::endl;
04506         return false;
04507       }
04508     
04509     }
04510   
04511     /* If multiple sectors are defined */
04512     if (num_sectors > 1) {
04513 
04514       /* Check whether the given sector arrangement is overlapping */
04515       for (unsigned int i = 0; i < (num_sectors - 1); i++) {
04516         if (sector_start_angles[i] > sector_stop_angles[i] || sector_stop_angles[i] >= sector_start_angles[i+1]) {
04517           std::cerr << "Invalid sector definitions! (check sector bounds)" << std::endl;
04518           return false;
04519         }    
04520       }
04521     
04522       /* Check the last sector against the first */    
04523       if (sector_stop_angles[num_sectors-1] <= sector_start_angles[num_sectors-1] &&
04524           sector_stop_angles[num_sectors-1] >= sector_start_angles[0]) {
04525         std::cerr << "Invalid sector definitions! (check sector bounds)" << std::endl;
04526         return false;
04527       }
04528     
04529     }
04530 
04531     /* Valid! */
04532     return true;
04533   }
04534 
04540   bool SickLD::_supportedScanProfileFormat( const uint16_t profile_format ) const {
04541 
04542     /* Check the supplied scan profile format */
04543     switch(profile_format) {
04544     case SICK_SCAN_PROFILE_RANGE:
04545       return true;
04546     case SICK_SCAN_PROFILE_RANGE_AND_ECHO:
04547       return true;
04548     default:
04549       return false;
04550     }
04551   }
04552 
04557   void SickLD::_printSectorProfileData( const sick_ld_sector_data_t &sector_data ) const {
04558   
04559     std::cout << "\t---- Sector Data " << sector_data.sector_num << " ----" << std::endl;
04560     std::cout << "\tSector Num.: " <<  sector_data.sector_num << std::endl;
04561     std::cout << "\tSector Angle Step (deg): " << sector_data.angle_step<< std::endl;
04562     std::cout << "\tSector Num. Data Points: " << sector_data.num_data_points << std::endl;
04563     std::cout << "\tSector Start Timestamp (ms): " << sector_data.timestamp_start << std::endl;
04564     std::cout << "\tSector Stop Timestamp (ms): " << sector_data.timestamp_stop << std::endl;
04565     std::cout << "\tSector Start Angle (deg): " << sector_data.angle_start << std::endl;
04566     std::cout << "\tSector Stop Angle (deg): " << sector_data.angle_stop << std::endl;
04567     std::cout << std::flush;
04568   }
04569 
04576   void SickLD::_printSickScanProfile( const sick_ld_scan_profile_t profile_data, const bool print_sector_data ) const {
04577   
04578     std::cout << "\t========= Sick Scan Prof. =========" << std::endl;
04579     std::cout << "\tProfile Num.: " << profile_data.profile_number << std::endl;
04580     std::cout << "\tProfile Counter: " << profile_data.profile_counter << std::endl;
04581     std::cout << "\tLayer Num.: " << profile_data.layer_num << std::endl;
04582     std::cout << "\tSensor Status: " <<  _sickSensorModeToString(profile_data.sensor_status) << std::endl;
04583     std::cout << "\tMotor Status: " <<  _sickMotorModeToString(profile_data.motor_status) << std::endl;
04584     std::cout << "\tNum. Sectors: " << profile_data.num_sectors << std::endl;
04585 
04586     // Print the corresponding active and non-active sector data
04587     for (unsigned int i=0; i < profile_data.num_sectors && print_sector_data; i++) {
04588       _printSectorProfileData(profile_data.sector_data[i]);
04589     }
04590     
04591     std::cout << "\t====================================" << std::endl;
04592     std::cout << std::flush;
04593   }
04594 
04600   uint8_t SickLD::_sickSensorModeToWorkServiceSubcode( const uint8_t sick_sensor_mode ) const {
04601 
04602     switch(sick_sensor_mode) {
04603     case SICK_SENSOR_MODE_IDLE:
04604       return SICK_WORK_SERV_TRANS_IDLE;
04605     case SICK_SENSOR_MODE_ROTATE:
04606       return SICK_WORK_SERV_TRANS_ROTATE;
04607     case SICK_SENSOR_MODE_MEASURE:
04608       return SICK_WORK_SERV_TRANS_MEASURE;
04609     default:
04610       std::cerr << "SickLD::_sickSensorModeToWorkServiceSubcode: Invalid sensor mode! (Returning 0)" << std::endl;
04611       return 0; //Something is seriously wrong if we end up here!
04612     }
04613   }
04614 
04620   std::string SickLD::_sickSensorModeToString( const uint8_t sick_sensor_mode ) const {
04621 
04622     switch(sick_sensor_mode) {
04623     case SICK_SENSOR_MODE_IDLE:
04624       return "IDLE";
04625     case SICK_SENSOR_MODE_ROTATE:
04626       return "ROTATE (laser is off)";
04627     case SICK_SENSOR_MODE_MEASURE:
04628       return "MEASURE (laser is on)";
04629     case SICK_SENSOR_MODE_ERROR:
04630       return "ERROR";
04631     case SICK_SENSOR_MODE_UNKNOWN:
04632       return "UNKNOWN";
04633     default:
04634       return "UNRECOGNIZED!!!";
04635     }
04636   }
04637 
04643   std::string SickLD::_sickMotorModeToString( const uint8_t sick_motor_mode ) const {
04644 
04645     switch(sick_motor_mode) {
04646     case SICK_MOTOR_MODE_OK:
04647       return "OK";
04648     case SICK_MOTOR_MODE_SPIN_TOO_HIGH:
04649       return "SPIN TOO HIGH";
04650     case SICK_MOTOR_MODE_SPIN_TOO_LOW:
04651       return "SPIN TOO LOW";
04652     case SICK_MOTOR_MODE_ERROR:
04653       return "ERROR";
04654     case SICK_MOTOR_MODE_UNKNOWN:
04655       return "UNKNOWN";
04656     default:
04657       return "UNRECOGNIZED!!!";
04658     }
04659   }
04660 
04666   std::string SickLD::_sickTransMeasureReturnToString( const uint8_t return_value ) const {
04667 
04668     switch(return_value) {
04669     case SICK_WORK_SERV_TRANS_MEASURE_RET_OK:
04670       return "LD-OEM/LD-LRS Measures";
04671     case SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE:
04672       return "Max Pulse Frequency Too High";
04673     case SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE:
04674       return "Mean Pulse Frequency Too High";
04675     case SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER:
04676       return "Sector Borders Not Configured Correctly";
04677     case SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT:
04678       return "Sector Borders Not Multiple of Angle Step";
04679     default:
04680       return "UNRECOGNIZED!!!";
04681     }
04682   
04683   }
04684 
04690   std::string SickLD::_sickResetLevelToString( const uint16_t reset_level ) const {
04691 
04692     switch(reset_level) {
04693     case SICK_WORK_SERV_RESET_INIT_CPU:
04694       return "RESET (CPU Reinitialized)";
04695     case SICK_WORK_SERV_RESET_KEEP_CPU:
04696       return "RESET (CPU Not Reinitialized)";
04697     case SICK_WORK_SERV_RESET_HALT_APP:
04698       return "RESET (Halt App. and Enter IDLE)";
04699     default:
04700       return "UNRECOGNIZED!!!";
04701     }
04702   
04703   }
04704 
04710   std::string SickLD::_sickSectorFunctionToString( const uint16_t sick_sector_function ) const {
04711 
04712     switch(sick_sector_function) {
04713     case SICK_CONF_SECTOR_NOT_INITIALIZED:
04714       return "NOT INITIALIZED";
04715     case SICK_CONF_SECTOR_NO_MEASUREMENT:
04716       return "NOT MEASURING";
04717     case SICK_CONF_SECTOR_RESERVED:
04718       return "RESERVED";
04719     case SICK_CONF_SECTOR_NORMAL_MEASUREMENT:
04720       return "MEASURING";
04721     case SICK_CONF_SECTOR_REFERENCE_MEASUREMENT:
04722       return "REFERENCE";
04723     default:
04724       return "UNRECOGNIZED!!!";
04725     }
04726   
04727   }
04728 
04734   std::string SickLD::_sickProfileFormatToString( const uint16_t profile_format ) const {
04735 
04736     switch(profile_format) {
04737     case SICK_SCAN_PROFILE_RANGE:
04738       return "RANGE ONLY";
04739     case SICK_SCAN_PROFILE_RANGE_AND_ECHO:
04740       return "RANGE + ECHO";
04741     default:
04742       return "UNRECOGNIZED!!!";
04743     }
04744   
04745   }
04746 
04750   void SickLD::_printInitFooter( ) const {
04751 
04752     std::cout << "\t*** Init. complete: Sick LD is online and ready!" << std::endl; 
04753     std::cout << "\tNum. Active Sectors: " << (int)_sick_sector_config.sick_num_active_sectors << std::endl;  
04754     std::cout << "\tMotor Speed: " << _sick_global_config.sick_motor_speed << " (Hz)" << std::endl;
04755     std::cout << "\tScan Resolution: " << _sick_global_config.sick_angle_step << " (deg)" << std::endl;
04756     std::cout << std::endl;
04757     
04758   }
04759 
04760 } //namespace SickToolbox


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