SickPLS.cc
Go to the documentation of this file.
00001 
00016 /* Auto-generated header */
00017 //#include "SickConfig.hh"
00018 
00019 /* Implementation dependencies */
00020 #include <sstream>
00021 #include <iostream>
00022 #include <termios.h>
00023 #include <sys/ioctl.h>
00024 #include <signal.h>
00025 
00026 #include "SickPLS.hh"
00027 #include "SickPLSMessage.hh"
00028 #include "SickPLSBufferMonitor.hh"
00029 #include "SickPLSUtility.hh"
00030 #include "SickException.hh"
00031 
00032 #ifdef HAVE_LINUX_SERIAL_H
00033 #include <linux/serial.h>
00034 #else
00035 #define B500000 0010005
00036 #endif
00037 
00038 /* Associate the namespace */
00039 namespace SickToolbox
00040 {
00041 
00046 SickPLS::SickPLS( const std::string sick_device_path ): SickLIDAR< SickPLSBufferMonitor, SickPLSMessage >( ),
00047     _sick_device_path(sick_device_path),
00048     _curr_session_baud(SICK_BAUD_UNKNOWN),
00049     _desired_session_baud(SICK_BAUD_UNKNOWN)
00050 {
00051 
00052     /* Initialize the protected/private structs */
00053     memset(&_sick_operating_status,0,sizeof(sick_pls_operating_status_t));
00054     memset(&_sick_baud_status,0,sizeof(sick_pls_baud_status_t));
00055     memset(&_old_term,0,sizeof(struct termios));
00056 
00057     //start in an unknown mode
00058     _sick_operating_status.sick_operating_mode  = SICK_OP_MODE_UNKNOWN;
00059 
00060     //fixed parameters
00061     _sick_operating_status.sick_measuring_units = SICK_MEASURING_UNITS_CM;
00062     _sick_operating_status.sick_scan_resolution = SICK_SCAN_RESOLUTION_50;
00063     _sick_operating_status.sick_scan_angle = SICK_SCAN_ANGLE_180;
00064 }
00065 
00069 SickPLS::~SickPLS()
00070 {
00071 
00072     try
00073     {
00074 
00075         /* Attempt to uninitialize the device */
00076         _teardownConnection();
00077 
00078     }
00079 
00080     /* Catch an I/O exception */
00081     catch(SickIOException &sick_io_exception)
00082     {
00083         std::cerr << sick_io_exception.what() << std::endl;
00084     }
00085 
00086     /* Catch anything else */
00087     catch(...)
00088     {
00089         std::cerr << "SickPLS::~SickPLS: Unknown exception!" << std::endl;
00090     }
00091 
00092 }
00093 
00099 bool SickPLS::Initialize( const sick_pls_baud_t desired_baud_rate )
00100 throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException )
00101 {
00102 
00103     /* Buffer the desired baud rate in case we have to reset */
00104     _desired_session_baud = desired_baud_rate;
00105 
00106     try
00107     {
00108 
00109         std::cout << std::endl << "\t*** Attempting to initialize the Sick PLS..." << std::endl << std::flush;
00110 
00111         /* Initialize the serial term for communication */
00112         std::cout << "\tAttempting to open device @ " << _sick_device_path << std::endl << std::flush;
00113         _setupConnection();
00114         std::cout << "\t\tDevice opened!" << std::endl << std::flush;
00115 
00116         /* Start/reset the buffer monitor */
00117         if (!_sick_monitor_running)
00118         {
00119             std::cout << "\tAttempting to start buffer monitor..." << std::endl;
00120             _startListening();
00121             std::cout << "\t\tBuffer monitor started!" << std::endl;
00122         }
00123         else
00124         {
00125             std::cout << "\tAttempting to reset buffer monitor..." << std::endl;
00126             _sick_buffer_monitor->SetDataStream(_sick_fd);
00127             std::cout << "\t\tBuffer monitor reset!" << std::endl;
00128         }
00129 
00130         try
00131         {
00132 
00133             std::cout << "\tAttempting to set session baud rate to "
00134                       << SickPLS::SickBaudToString(_desired_session_baud)
00135                       << " as requested..." << std::endl;
00136             _setSessionBaud(_desired_session_baud);
00137 
00138         }
00139 
00140         /* Assume a timeout is due to a misconfigured terminal baud */
00141         catch(SickTimeoutException &sick_timeout)
00142         {
00143 
00144             /* Check whether to do an autodetect */
00145             sick_pls_baud_t default_baud = _baudToSickBaud(DEFAULT_SICK_PLS_SICK_BAUD);
00146             std::cout << "\tFailed to set requested baud rate..." << std::endl << std::flush;
00147             std::cout << "\tAttempting to detect PLS baud rate..." << std::endl << std::flush;
00148             if((default_baud != SICK_BAUD_38400) && _testSickBaud(SICK_BAUD_38400))
00149             {
00150                 std::cout << "\t\tDetected PLS baud @ " << SickBaudToString(SICK_BAUD_38400) << "!" << std::endl;
00151             }
00152             else if((default_baud) != SICK_BAUD_500K && _testSickBaud(SICK_BAUD_500K))
00153             {
00154                 std::cout << "\t\tDetected PLS baud @ " << SickBaudToString(SICK_BAUD_500K) << "!" << std::endl;
00155             }
00156             else if((default_baud != SICK_BAUD_9600) && _testSickBaud(SICK_BAUD_9600))
00157             {
00158                 std::cout << "\t\tDetected PLS baud @ " << SickBaudToString(SICK_BAUD_9600) << "!" << std::endl;
00159             }
00160             else if((default_baud != SICK_BAUD_19200) && _testSickBaud(SICK_BAUD_19200))
00161             {
00162                 std::cout << "\t\tDetected PLS baud @ " << SickBaudToString(SICK_BAUD_19200) << "!" << std::endl;
00163             }
00164             else
00165             {
00166                 _stopListening();
00167                 return false;
00168                 throw SickIOException("SickPLS::Initialize: failed to detect baud rate!");
00169             }
00170             std::cout << std::flush;
00171 
00172             /* Try again! */
00173             if (_curr_session_baud != _desired_session_baud)
00174             {
00175                 std::cout << "\tAttempting to setup desired baud (again)..." << std::endl << std::flush;
00176                 _setSessionBaud(_desired_session_baud);
00177             }
00178 
00179         }
00180 
00181         /* Catch anything else */
00182         catch(...)
00183         {
00184             return false;
00185             std::cerr << "SickPLS::Initialize: Unknown exception!" << std::endl;
00186             throw;
00187         }
00188 
00189         std::cout << "\t\tOperating @ " << SickBaudToString(_curr_session_baud) << std::endl;
00190 
00191         /* Set the device to request range mode */
00192         _setSickOpModeMonitorRequestValues();
00193 
00194 
00195         /* Set the flag */
00196         _sick_initialized = true;
00197 
00198     }
00199 
00200     /* Handle a config exception */
00201     catch(SickConfigException &sick_config_exception)
00202     {
00203         return false;
00204         std::cerr << sick_config_exception.what() << std::endl;
00205         throw;
00206     }
00207 
00208     /* Handle a timeout exception */
00209     catch(SickTimeoutException &sick_timeout_exception)
00210     {
00211         return false;
00212         std::cerr << sick_timeout_exception.what() << std::endl;
00213         throw;
00214     }
00215 
00216     /* Handle any I/O exceptions */
00217     catch(SickIOException &sick_io_exception)
00218     {
00219         return false;
00220         std::cerr << sick_io_exception.what() << std::endl;
00221         throw;
00222     }
00223 
00224     /* Handle any thread exceptions */
00225     catch(SickThreadException &sick_thread_exception)
00226     {
00227         return false;
00228         std::cerr << sick_thread_exception.what() << std::endl;
00229         throw;
00230     }
00231 
00232     /* Handle anything else */
00233     catch(...)
00234     {
00235         return false;
00236         std::cerr << "SickPLS::Initialize: Unknown exception!" << std::endl;
00237         throw;
00238     }
00239 
00240     /* Initialization was successful! */
00241     std::cout << "\t*** Init. complete: Sick PLS is online and ready!" << std::endl;
00242     std::cout << "\tScan Angle: " << GetSickScanAngle() << " (deg)" << std::endl;
00243     std::cout << "\tScan Resolution: " << GetSickScanResolution() << " (deg)" << std::endl;
00244     std::cout << "\tMeasuring Units: " << SickMeasuringUnitsToString(GetSickMeasuringUnits()) << std::endl;
00245     std::cout << std::endl << std::flush;
00246     return true;
00247 }
00248 
00253 void SickPLS::Uninitialize( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException )
00254 {
00255 
00256     if (_sick_initialized)
00257     {
00258 
00259         std::cout << std::endl << "\t*** Attempting to uninitialize the Sick PLS..." << std::endl;
00260 
00261         try
00262         {
00263 
00264             /* Restore original operating mode */
00265             _setSickOpModeMonitorRequestValues();
00266 
00267             /* Restore original baud rate settings */
00268             _setSessionBaud(_baudToSickBaud(DEFAULT_SICK_PLS_SICK_BAUD));
00269 
00270             /* Attempt to cancel the buffer monitor */
00271             if (_sick_monitor_running)
00272             {
00273                 std::cout << "\tAttempting to stop buffer monitor..." << std::endl;
00274                 _stopListening();
00275                 std::cout << "\t\tBuffer monitor stopped!" << std::endl;
00276             }
00277 
00278             std::cout << "\t*** Uninit. complete - Sick PLS is now offline!" << std::endl << std::flush;
00279 
00280         }
00281 
00282         /* Handle any config exceptions */
00283         catch(SickConfigException &sick_config_exception)
00284         {
00285             std::cerr << sick_config_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00286             throw;
00287         }
00288 
00289         /* Handle a timeout exception */
00290         catch(SickTimeoutException &sick_timeout_exception)
00291         {
00292             std::cerr << sick_timeout_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00293             throw;
00294         }
00295 
00296         /* Handle any I/O exceptions */
00297         catch(SickIOException &sick_io_exception)
00298         {
00299             std::cerr << sick_io_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00300             throw;
00301         }
00302 
00303         /* Handle any thread exceptions */
00304         catch(SickThreadException &sick_thread_exception)
00305         {
00306             std::cerr << sick_thread_exception.what() << " (attempting to kill connection anyways)" << std::endl;
00307             throw;
00308         }
00309 
00310         /* Handle anything else */
00311         catch(...)
00312         {
00313             std::cerr << "SickPLS::Unintialize: Unknown exception!!!" << std::endl;
00314             throw;
00315         }
00316 
00317         /* Reset the flag */
00318         _sick_initialized = false;
00319 
00320     }
00321 
00322 }
00323 
00328 std::string SickPLS::GetSickDevicePath( ) const
00329 {
00330     return _sick_device_path;
00331 }
00332 
00333 
00338 double SickPLS::GetSickScanAngle( ) const throw( SickConfigException )
00339 {
00340 
00341     /* Ensure the device is initialized */
00342     if (!_sick_initialized)
00343     {
00344         throw SickConfigException("SickPLS::GetSickScanAngle: Sick PLS is not initialized!");
00345     }
00346 
00347     /* Return the Sick scan angle */
00348     return (double)_sick_operating_status.sick_scan_angle;
00349 
00350 }
00351 
00356 double SickPLS::GetSickScanResolution( ) const throw( SickConfigException )
00357 {
00358 
00359     /* Ensure the device is initialized */
00360     if (!_sick_initialized)
00361     {
00362         throw SickConfigException("SickPLS::GetSickScanResolution: Sick PLS is not initialized!");
00363     }
00364 
00365     /* Return the scan resolution */
00366     return _sick_operating_status.sick_scan_resolution*(0.01);
00367 
00368 }
00369 
00370 
00371 
00376 sick_pls_measuring_units_t SickPLS::GetSickMeasuringUnits( ) const throw( SickConfigException )
00377 {
00378 
00379     /* Ensure the device is initialized */
00380     if (!_sick_initialized)
00381     {
00382         throw SickConfigException("SickPLS::GetSickMeasuringUnits: Sick PLS is not initialized!");
00383     }
00384 
00385     return (sick_pls_measuring_units_t)_sick_operating_status.sick_measuring_units;
00386 }
00387 
00392 sick_pls_operating_mode_t SickPLS::GetSickOperatingMode( ) const throw( SickConfigException )
00393 {
00394 
00395     /* Ensure the device is initialized */
00396     if (!_sick_initialized)
00397     {
00398         throw SickConfigException("SickPLS::GetSickScanAngle: Sick PLS is not initialized!");
00399     }
00400 
00401     /* Return the current operating mode of the device */
00402     return (sick_pls_operating_mode_t)_sick_operating_status.sick_operating_mode;
00403 
00404 }
00405 
00406 
00407 
00424 void SickPLS::GetSickScan( unsigned int * const measurement_values,
00425                            unsigned int & num_measurement_values) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException)
00426 {
00427 
00428     /* Ensure the device is initialized */
00429     if (!_sick_initialized)
00430     {
00431         throw SickConfigException("SickPLS::GetSickScan: Sick PLS is not initialized!");
00432     }
00433 
00434     /* Declare message objects */
00435     SickPLSMessage response;
00436 
00437     /* Declare some useful variables and a buffer */
00438     uint8_t payload_buffer[SickPLSMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00439 
00440     try
00441     {
00442 
00443         /* Restore original operating mode */
00444         _setSickOpModeMonitorStreamValues();
00445 
00446         /* Receive a data frame from the stream. */
00447         _recvMessage(response,DEFAULT_SICK_PLS_SICK_MESSAGE_TIMEOUT);
00448 
00449         /* Check that our payload has the proper command byte of 0xB0 */
00450         if(response.GetCommandCode() != 0xB0)
00451         {
00452             throw SickIOException("SickPLS::GetSickScan: Unexpected message!");
00453         }
00454 
00455         /* Acquire the payload buffer and length*/
00456         response.GetPayload(payload_buffer);
00457 
00458         /* Define a local scan profile object */
00459         sick_pls_scan_profile_b0_t sick_scan_profile;
00460 
00461         /* Initialize the profile */
00462         memset(&sick_scan_profile,0,sizeof(sick_pls_scan_profile_b0_t));
00463 
00464         /* Parse the message payload */
00465         _parseSickScanProfileB0(&payload_buffer[1],sick_scan_profile);
00466 
00467         /* Return the request values! */
00468         num_measurement_values = sick_scan_profile.sick_num_measurements;
00469 
00470         for (unsigned int i = 0; i < num_measurement_values; i++)
00471         {
00472 
00473             /* Copy the measurement value */
00474             measurement_values[i] = sick_scan_profile.sick_measurements[i];
00475 
00476         }
00477 
00478     }
00479 
00480     /* Handle any config exceptions */
00481     catch(SickConfigException &sick_config_exception)
00482     {
00483         std::cerr << sick_config_exception.what() << std::endl;
00484         throw;
00485     }
00486 
00487     /* Handle a timeout exception */
00488     catch(SickTimeoutException &sick_timeout_exception)
00489     {
00490         std::cerr << sick_timeout_exception.what() << std::endl;
00491         throw;
00492     }
00493 
00494     /* Handle any I/O exceptions */
00495     catch(SickIOException &sick_io_exception)
00496     {
00497         std::cerr << sick_io_exception.what() << std::endl;
00498         throw;
00499     }
00500 
00501     /* Handle any thread exceptions */
00502     catch(SickThreadException &sick_thread_exception)
00503     {
00504         std::cerr << sick_thread_exception.what() << std::endl;
00505         throw;
00506     }
00507 
00508     /* Handle anything else */
00509     catch(...)
00510     {
00511         std::cerr << "SickPLS::GetSickScan: Unknown exception!!!" << std::endl;
00512         throw;
00513     }
00514 
00515 }
00516 
00517 
00518 
00519 
00520 
00525 void SickPLS::ResetSick( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException )
00526 {
00527 
00528     /* Ensure the device is initialized */
00529     if (!_sick_initialized)
00530     {
00531         throw SickConfigException("SickPLS::ResetSick: Sick PLS is not initialized!");
00532     }
00533 
00534     SickPLSMessage message,response;
00535     uint8_t payload[SickPLSMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
00536 
00537     /* Construct the reset command */
00538     payload[0] = 0x10; // Request field reset
00539     message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload,1);
00540 
00541     std::cout << "\tResetting the device..." << std::endl;
00542     std::cout << "\tWaiting for Power on message..." << std::endl;
00543 
00544     try
00545     {
00546 
00547         /* Send the reset command and wait for the reply */
00548         _sendMessageAndGetReply(message,response,0x91,(unsigned int)60e6,DEFAULT_SICK_PLS_NUM_TRIES);
00549 
00550         std::cout << "\t\tPower on message received!" << std::endl;
00551         std::cout << "\tWaiting for PLS Ready message..." << std::endl;
00552 
00553         /* Set terminal baud to the detected rate to get the PLS ready message */
00554         _setTerminalBaud(_baudToSickBaud(DEFAULT_SICK_PLS_SICK_BAUD));
00555 
00556         /* Receive the PLS ready message after power on */
00557         _recvMessage(response,(unsigned int)30e6);
00558 
00559         /* Verify the response */
00560         if(response.GetCommandCode() != 0x90)
00561         {
00562             std::cerr << "SickPLS::ResetSick: Unexpected reply! (assuming device has been reset!)"  << std::endl;
00563         }
00564         else
00565         {
00566             std::cout << "\t\tPLS Ready message received!" << std::endl;
00567         }
00568         std::cout << std::endl;
00569 
00570         /* Reinitialize and sync the device */
00571         Initialize(_desired_session_baud);
00572 
00573     }
00574 
00575     /* Catch any timeout exceptions */
00576     catch(SickTimeoutException &sick_timeout_exception)
00577     {
00578         std::cerr << sick_timeout_exception.what() << std::endl;
00579         throw;
00580     }
00581 
00582     /* Catch any I/O exceptions */
00583     catch(SickIOException &sick_io_exception)
00584     {
00585         std::cerr << sick_io_exception.what() << std::endl;
00586         throw;
00587     }
00588 
00589     /* Catch any thread exceptions */
00590     catch(SickThreadException &sick_thread_exception)
00591     {
00592         std::cerr << sick_thread_exception.what() << std::endl;
00593         throw;
00594     }
00595 
00596     /* Catch anything else */
00597     catch(...)
00598     {
00599         std::cerr << "SickPLS::ResetSick: Unknown exception!!!" << std::endl;
00600         throw;
00601     }
00602 
00603     std::cout << "\tRe-initialization sucessful. PLS is ready to go!" << std::endl;
00604 
00605 }
00606 
00611 std::string SickPLS::GetSickStatusAsString( ) const
00612 {
00613 
00614     std::stringstream str_stream;
00615 
00616     str_stream << "\t=============== Sick PLS Status ===============" << std::endl;
00617 
00618     /* If Sick is initialized then print the status! */
00619     if (_sick_initialized)
00620     {
00621 
00622         str_stream << "\tScan Angle: " << GetSickScanAngle() << " (deg)" << std::endl;
00623         str_stream << "\tScan Resolution: " << GetSickScanResolution() << " (deg)" << std::endl;
00624         str_stream << "\tOperating Mode: " << SickOperatingModeToString(GetSickOperatingMode()) << std::endl;
00625         str_stream << "\tMeasuring Units: " << SickMeasuringUnitsToString(GetSickMeasuringUnits()) << std::endl;
00626 
00627     }
00628     else
00629     {
00630 
00631         str_stream << "\t Unknown (Device is not initialized)" << std::endl;
00632 
00633     }
00634 
00635     str_stream << "\t===============================================" << std::endl;
00636 
00637     return str_stream.str();
00638 }
00639 
00640 
00641 
00642 
00643 
00648 sick_pls_scan_angle_t SickPLS::IntToSickScanAngle( const int scan_angle_int )
00649 {
00650 
00651     switch(scan_angle_int)
00652     {
00653     case 180:
00654         return SICK_SCAN_ANGLE_180;
00655     default:
00656         return SICK_SCAN_ANGLE_UNKNOWN;
00657     }
00658 
00659 }
00660 
00665 sick_pls_scan_resolution_t SickPLS::IntToSickScanResolution( const int scan_resolution_int )
00666 {
00667 
00668     switch(scan_resolution_int)
00669     {
00670     case 50:
00671         return SICK_SCAN_RESOLUTION_50;
00672     default:
00673         return SICK_SCAN_RESOLUTION_UNKNOWN;
00674     }
00675 
00676 }
00677 
00682 sick_pls_scan_resolution_t SickPLS::DoubleToSickScanResolution( const double scan_resolution_double )
00683 {
00684     return IntToSickScanResolution((const int)(scan_resolution_double*100));
00685 }
00686 
00692 std::string SickPLS::SickBaudToString( const sick_pls_baud_t baud_rate )
00693 {
00694 
00695     switch(baud_rate)
00696     {
00697     case SICK_BAUD_9600:
00698         return "9600bps";
00699     case SICK_BAUD_19200:
00700         return "19200bps";
00701     case SICK_BAUD_38400:
00702         return "38400bps";
00703     case SICK_BAUD_500K:
00704         return "500Kbps";
00705     default:
00706         return "Unknown!";
00707     }
00708 
00709 }
00710 
00715 sick_pls_baud_t SickPLS::IntToSickBaud( const int baud_int )
00716 {
00717 
00718     switch(baud_int)
00719     {
00720     case 9600:
00721         return SICK_BAUD_9600;
00722     case 19200:
00723         return SICK_BAUD_19200;
00724     case 38400:
00725         return SICK_BAUD_38400;
00726     case 500000:
00727         return SICK_BAUD_500K;
00728     default:
00729         return SICK_BAUD_UNKNOWN;
00730     }
00731 
00732 }
00733 
00738 sick_pls_baud_t SickPLS::StringToSickBaud( const std::string baud_str )
00739 {
00740 
00741     int baud_int;
00742     std::istringstream input_stream(baud_str);
00743     input_stream >> baud_int;
00744 
00745     return IntToSickBaud(baud_int);
00746 
00747 }
00748 
00754 std::string SickPLS::SickStatusToString( const sick_pls_status_t sick_status )
00755 {
00756 
00757     /* Return a string */
00758     if(sick_status != SICK_STATUS_OK)
00759     {
00760         return "Error (possibly fatal)";
00761     }
00762     return "OK!";
00763 
00764 }
00765 
00766 
00772 std::string SickPLS::SickOperatingModeToString( const sick_pls_operating_mode_t sick_operating_mode )
00773 {
00774 
00775     switch(sick_operating_mode)
00776     {
00777     case SICK_OP_MODE_INSTALLATION:
00778         return "Installation Mode";
00779     case SICK_OP_MODE_DIAGNOSTIC:
00780         return "Diagnostic Mode";
00781     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUE_FOR_EACH_SEGMENT:
00782         return "Stream mim measured values for each segment";
00783     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VALUE_ON_OBJECT:
00784         return "Min measured value for each segment when object detected";
00785     case SICK_OP_MODE_MONITOR_STREAM_MIN_VERT_DIST_TO_OBJECT:
00786         return "Min vertical distance";
00787     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VERT_DIST_TO_OBJECT:
00788         return "Min vertical distance when object detected";
00789     case SICK_OP_MODE_MONITOR_STREAM_VALUES:
00790         return "Stream all measured values";
00791     case SICK_OP_MODE_MONITOR_REQUEST_VALUES:
00792         return "Request measured values";
00793     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES:
00794         return "Stream mean measured values";
00795     case SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE:
00796         return "Stream measured value subrange";
00797     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE:
00798         return "Stream mean measured value subrange";
00799     case SICK_OP_MODE_MONITOR_STREAM_VALUES_WITH_FIELDS:
00800         return "Stream measured and field values";
00801     case SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN:
00802         return "Stream measured values from partial scan";
00803     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT_FROM_PARTIAL_SCAN:
00804         return "Stream range w/ reflectivity from partial scan";
00805     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUES_FOR_EACH_SEGMENT_SUBRANGE:
00806         return "Stream min measured values for each segment over a subrange";
00807     case SICK_OP_MODE_MONITOR_NAVIGATION:
00808         return "Output navigation data records";
00809     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT:
00810         return "Stream range w/ reflectivity values";
00811     default:
00812         return "Unknown!";
00813     };
00814 
00815 }
00816 
00817 
00818 
00824 std::string SickPLS::SickMeasuringUnitsToString( const sick_pls_measuring_units_t sick_units )
00825 {
00826 
00827     /* Return the proper string */
00828     switch(sick_units)
00829     {
00830     case SICK_MEASURING_UNITS_CM:
00831         return "Centimeters (cm)";
00832     default:
00833         return "Unknown!";
00834     }
00835 
00836 }
00837 
00841 void SickPLS::_setupConnection( ) throw ( SickIOException, SickThreadException )
00842 {
00843 
00844     try
00845     {
00846 
00847         /* Open the device */
00848         if((_sick_fd = open(_sick_device_path.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)) < 0)
00849         {
00850             throw SickIOException("SickPLS::_setupConnection: - Unable to open serial port");
00851         }
00852 
00853         /* Backup the original term settings */
00854         if(tcgetattr(_sick_fd,&_old_term) < 0)
00855         {
00856             throw SickIOException("SickPLS::_setupConnection: tcgetattr() failed!");
00857         }
00858 
00859         /* Set the host terminal baud rate to the new speed */
00860         _setTerminalBaud(_baudToSickBaud(DEFAULT_SICK_PLS_SICK_BAUD));
00861 
00862     }
00863 
00864     /* Handle any I/O exceptions */
00865     catch(SickIOException &sick_io_exception)
00866     {
00867         std::cerr << sick_io_exception.what() << std::endl;
00868         throw;
00869     }
00870 
00871     /* Handle any thread exceptions */
00872     catch(SickThreadException &sick_thread_exception)
00873     {
00874         std::cerr << sick_thread_exception.what() << std::endl;
00875         throw;
00876     }
00877 
00878     /* Handle unknown exceptions */
00879     catch(...)
00880     {
00881         std::cerr << "SickPLS::_setupConnection: Unknown exception!" << std::endl;
00882         throw;
00883     }
00884 
00885 }
00886 
00890 void SickPLS::_teardownConnection( ) throw( SickIOException )
00891 {
00892 
00893     /* Check whether device was initialized */
00894     if(!_sick_initialized)
00895     {
00896         return;
00897     }
00898 
00899     /* Restore old terminal settings */
00900     if (tcsetattr(_sick_fd,TCSANOW,&_old_term) < 0)
00901     {
00902         throw SickIOException("SickPLS::_teardownConnection: tcsetattr() failed!");
00903     }
00904 
00905     /* Actually close the device */
00906     if(close(_sick_fd) != 0)
00907     {
00908         throw SickIOException("SickPLS::_teardownConnection: close() failed!");
00909     }
00910 
00911 }
00912 
00916 void SickPLS::_flushTerminalBuffer( ) throw ( SickThreadException )
00917 {
00918 
00919     try
00920     {
00921 
00922         /* Acquire access to the data stream */
00923         _sick_buffer_monitor->AcquireDataStream();
00924 
00925         /* Nobody is reading a message, so safely flush! */
00926         if (tcflush(_sick_fd,TCIOFLUSH) != 0)
00927         {
00928             throw SickThreadException("SickPLS::_flushTerminalBuffer: tcflush() failed!");
00929         }
00930 
00931         /* Attempt to release the data stream */
00932         _sick_buffer_monitor->ReleaseDataStream();
00933 
00934     }
00935 
00936     /* Handle thread exceptions */
00937     catch(SickThreadException &sick_thread_exception)
00938     {
00939         std::cerr << sick_thread_exception.what() << std::endl;
00940         throw;
00941     }
00942 
00943     /* A sanity check */
00944     catch(...)
00945     {
00946         std::cerr << "SickPLS::_flushTerminalBuffer: Unknown exception!" << std::endl;
00947         throw;
00948     }
00949 
00950 }
00951 
00962 void SickPLS::_sendMessageAndGetReply( const SickPLSMessage &send_message,
00963                                        SickPLSMessage &recv_message,
00964                                        const unsigned int timeout_value,
00965                                        const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException )
00966 {
00967 
00968     uint8_t sick_reply_code = send_message.GetCommandCode() + 0x80;
00969 
00970     // std::cout.setf(std::ios::hex,std::ios::basefield);
00971     // std::cout << "Waiting for reply code: " << (unsigned int) sick_reply_code << std::endl;
00972     // std::cout.setf(std::ios::dec,std::ios::basefield);
00973 
00974 
00975     try
00976     {
00977 
00978         /* Send a message and get reply using a reply code */
00979         _sendMessageAndGetReply(send_message,recv_message,sick_reply_code,timeout_value,num_tries);
00980 
00981     }
00982 
00983     /* Handle a timeout! */
00984     catch (SickTimeoutException &sick_timeout)
00985     {
00986         /* For now just rethrow it */
00987         throw;
00988     }
00989 
00990     /* Handle a thread exception */
00991     catch (SickThreadException &sick_thread_exception)
00992     {
00993         std::cerr << sick_thread_exception.what() << std::endl;
00994         throw;
00995     }
00996 
00997     /* Handle write buffer exceptions */
00998     catch (SickIOException &sick_io_error)
00999     {
01000         std::cerr << sick_io_error.what() << std::endl;
01001         throw;
01002     }
01003 
01004     /* A safety net */
01005     catch (...)
01006     {
01007         std::cerr << "SickPLS::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
01008         throw;
01009     }
01010 
01011 }
01012 
01021 void SickPLS::_sendMessageAndGetReply( const SickPLSMessage &send_message,
01022                                        SickPLSMessage &recv_message,
01023                                        const uint8_t reply_code,
01024                                        const unsigned int timeout_value,
01025                                        const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException )
01026 {
01027 
01028     try
01029     {
01030 
01031         /* Attempt to flush the terminal buffer */
01032         _flushTerminalBuffer();
01033 
01034         /* Send a message and get reply using parent's method */
01035         SickLIDAR< SickPLSBufferMonitor, SickPLSMessage >::_sendMessageAndGetReply(send_message,
01036                 recv_message,
01037                 &reply_code, 1,
01038                 DEFAULT_SICK_PLS_BYTE_INTERVAL,
01039                 timeout_value, num_tries);
01040 
01041     }
01042 
01043     /* Handle a timeout! */
01044     catch (SickTimeoutException &sick_timeout)
01045     {
01046         throw;
01047     }
01048 
01049     /* Handle a thread exception */
01050     catch (SickThreadException &sick_thread_exception)
01051     {
01052         std::cerr << sick_thread_exception.what() << std::endl;
01053         throw;
01054     }
01055 
01056     /* Handle write buffer exceptions */
01057     catch (SickIOException &sick_io_error)
01058     {
01059         std::cerr << sick_io_error.what() << std::endl;
01060         throw;
01061     }
01062 
01063     /* A safety net */
01064     catch (...)
01065     {
01066         std::cerr << "SickPLS::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
01067         throw;
01068     }
01069 
01070 }
01071 
01076 void SickPLS::_setSessionBaud(const sick_pls_baud_t baud_rate) throw ( SickIOException, SickThreadException, SickTimeoutException )
01077 {
01078 
01079     //PLS-specific before session baud can be set, we need to enter setup mode (setup in PLS software, called installation in SickToolbox)
01080 
01081     _setSickOpModeInstallation();
01082 
01083     std::cout<< "Setting session baud from operating mode: "
01084              << _sick_operating_status.sick_operating_mode
01085              << std::endl;
01086 
01087     SickPLSMessage message, response;
01088 
01089     uint8_t payload[SickPLSMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01090 
01091     /* Another sanity check */
01092     if(baud_rate == SICK_BAUD_UNKNOWN)
01093     {
01094         throw SickIOException("SickPLS::_setSessionBaud: Undefined baud rate!");
01095     }
01096 
01097     /* Construct the command telegram */
01098     payload[0] = 0x20;
01099     payload[1] = baud_rate;
01100 
01101     message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload,2);
01102 
01103     // message.Print();
01104 
01105     try
01106     {
01107 
01108         /* Send the status request and get a reply */
01109         _sendMessageAndGetReply(message,response,DEFAULT_SICK_PLS_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_PLS_NUM_TRIES);
01110 
01111         /* Set the host terminal baud rate to the new speed */
01112         _setTerminalBaud(baud_rate);
01113 
01114         /* Sick likes a sleep here */
01115         usleep(2500);
01116 
01117     }
01118 
01119     /* Catch a timeout */
01120     catch(SickTimeoutException &sick_timeout_exception)
01121     {
01122         std::cerr << sick_timeout_exception.what() << std::endl;
01123         throw;
01124     }
01125 
01126     /* Catch any I/O exceptions */
01127     catch(SickIOException &sick_io_exception)
01128     {
01129         std::cerr << sick_io_exception.what() << std::endl;
01130         throw;
01131     }
01132 
01133     /* Catch any thread exceptions */
01134     catch(SickThreadException &sick_thread_exception)
01135     {
01136         std::cerr << sick_thread_exception.what() << std::endl;
01137         throw;
01138     }
01139 
01140     /* Catch anything else */
01141     catch(...)
01142     {
01143         std::cerr << "SickPLS::_getSickErrors: Unknown exception!!!" << std::endl;
01144         throw;
01145     }
01146 
01147 }
01148 
01153 bool SickPLS::_testSickBaud(const sick_pls_baud_t baud_rate) throw( SickIOException, SickThreadException )
01154 {
01155 
01156     try
01157     {
01158 
01159         /* Another sanity check */
01160         if(baud_rate == SICK_BAUD_UNKNOWN)
01161         {
01162             throw SickIOException("SickPLS::_testBaudRate: Undefined baud rate!");
01163         }
01164 
01165         /* Attempt to get status information at the current baud */
01166         std::cout << "\t\tChecking " << SickBaudToString(baud_rate) << "..." << std::endl;
01167 
01168         /* Set the host terminal baud rate to the test speed */
01169         _setTerminalBaud(baud_rate);
01170 
01171         try
01172         {
01173 
01174             /* Check to see if the Sick replies! */
01175             _getSickErrors();
01176 
01177         }
01178 
01179         /* Catch a timeout exception */
01180         catch(SickTimeoutException &sick_timeout_exception)
01181         {
01182             /* This means that the current baud rate timed out! */
01183             return false;
01184         }
01185 
01186         /* Catch anything else and throw it away */
01187         catch(...)
01188         {
01189             std::cerr << "SickPLS::_testBaudRate: Unknown exception!" << std::endl;
01190             throw;
01191         }
01192 
01193     }
01194 
01195     /* Handle any IO exceptions */
01196     catch(SickIOException &sick_io_exception)
01197     {
01198         std::cerr << sick_io_exception.what() << std::endl;
01199         throw;
01200     }
01201 
01202     /* Handle thread exceptions */
01203     catch(SickThreadException &sick_thread_exception)
01204     {
01205         std::cerr << sick_thread_exception.what() << std::endl;
01206         throw;
01207     }
01208 
01209     /* A safety net */
01210     catch(...)
01211     {
01212         std::cerr << "SickPLS::_testBaudRate: Unknown exception!!!" << std::endl;
01213         throw;
01214     }
01215 
01216     /* Success! */
01217     return true;
01218 
01219 }
01220 
01225 void SickPLS::_setTerminalBaud( const sick_pls_baud_t baud_rate ) throw( SickIOException, SickThreadException )
01226 {
01227 
01228     struct termios term;
01229 
01230     term.c_iflag |= INPCK;
01231     term.c_iflag &= ~IXOFF;
01232     term.c_cflag |= PARENB;
01233 
01234     try
01235     {
01236         /* Attempt to acquire device attributes */
01237         if(tcgetattr(_sick_fd,&term) < 0)
01238         {
01239             throw SickIOException("SickPLS::_setTerminalBaud: Unable to get device attributes!");
01240         }
01241 
01242         /* Switch on the baud rate */
01243         switch(baud_rate)
01244         {
01245         case SICK_BAUD_9600:
01246         {
01247             cfmakeraw(&term);
01248 
01249             term.c_iflag |= INPCK;
01250             term.c_iflag &= ~IXOFF;
01251             term.c_cflag |= PARENB;
01252 
01253             cfsetispeed(&term,B9600);
01254             cfsetospeed(&term,B9600);
01255             break;
01256         }
01257         case SICK_BAUD_19200:
01258         {
01259             cfmakeraw(&term);
01260 
01261             term.c_iflag |= INPCK;
01262             term.c_iflag &= ~IXOFF;
01263             term.c_cflag |= PARENB;
01264 
01265             cfsetispeed(&term,B19200);
01266             cfsetospeed(&term,B19200);
01267             break;
01268         }
01269         case SICK_BAUD_38400:
01270         {
01271             cfmakeraw(&term);
01272 
01273             term.c_iflag |= INPCK;
01274             term.c_iflag &= ~IXOFF;
01275             term.c_cflag |= PARENB;
01276 
01277             cfsetispeed(&term,B38400);
01278             cfsetospeed(&term,B38400);
01279             break;
01280         }
01281         case SICK_BAUD_500K:
01282         {
01283             cfmakeraw(&term);
01284 
01285             term.c_iflag |= INPCK;
01286             term.c_iflag &= ~IXOFF;
01287             term.c_cflag |= PARENB;
01288 
01289             cfsetispeed(&term,B500000);
01290             cfsetospeed(&term,B500000);
01291             break;
01292         }
01293         default:
01294             throw SickIOException("SickPLS::_setTerminalBaud: Unknown baud rate!");
01295         }
01296 
01297         /* Attempt to set the device attributes */
01298         if(tcsetattr(_sick_fd,TCSAFLUSH,&term) < 0 )
01299         {
01300             throw SickIOException("SickPLS::_setTerminalBaud: Unable to set device attributes!");
01301         }
01302 
01303         /* Buffer the rate locally */
01304         _curr_session_baud = baud_rate;
01305 
01306         /* Attempt to flush the I/O buffers */
01307         _flushTerminalBuffer();
01308 
01309     } // try
01310 
01311     /* Catch an IO exception */
01312     catch(SickIOException sick_io_exception)
01313     {
01314         std::cerr << sick_io_exception.what() << std::endl;
01315         throw;
01316     }
01317 
01318     /* Catch an IO exception */
01319     catch(SickThreadException sick_thread_exception)
01320     {
01321         std::cerr << sick_thread_exception.what() << std::endl;
01322         throw;
01323     }
01324 
01325     /* A sanity check */
01326     catch(...)
01327     {
01328         std::cerr << "SickPLS::_setTerminalBaud: Unknown exception!!!" << std::endl;
01329         throw;
01330     }
01331 
01332 }
01333 
01334 
01335 
01339 void SickPLS::_getSickErrors( unsigned int * const num_sick_errors, uint8_t * const error_type_buffer,
01340                               uint8_t * const error_num_buffer ) throw( SickTimeoutException, SickIOException, SickThreadException )
01341 {
01342 
01343     SickPLSMessage message, response;
01344 
01345     int payload_length;
01346     uint8_t payload_buffer[SickPLSMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01347 
01348     /* The command to request PLS status */
01349     payload_buffer[0] = 0x32;
01350 
01351     /* Build the request message */
01352     message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,1);
01353 
01354     try
01355     {
01356 
01357         /* Send the status request and get a reply */
01358         _sendMessageAndGetReply(message,response,DEFAULT_SICK_PLS_SICK_MESSAGE_TIMEOUT,DEFAULT_SICK_PLS_NUM_TRIES);
01359 
01360     }
01361 
01362     /* Catch any timeout exceptions */
01363     catch(SickTimeoutException &sick_timeout_exception)
01364     {
01365         std::cerr << sick_timeout_exception.what() << std::endl;
01366         throw;
01367     }
01368 
01369     /* Catch any I/O exceptions */
01370     catch(SickIOException &sick_io_exception)
01371     {
01372         std::cerr << sick_io_exception.what() << std::endl;
01373         throw;
01374     }
01375 
01376     /* Catch any thread exceptions */
01377     catch(SickThreadException &sick_thread_exception)
01378     {
01379         std::cerr << sick_thread_exception.what() << std::endl;
01380         throw;
01381     }
01382 
01383     /* Catch anything else */
01384     catch(...)
01385     {
01386         std::cerr << "SickPLS::_getSickErrors: Unknown exception!!!" << std::endl;
01387         throw;
01388     }
01389 
01390     /* Extract the payload_length */
01391     payload_length = response.GetPayloadLength();
01392 
01393     /* Compute the number of errors */
01394     double num_errors = ((payload_length-2)/((double)2));
01395 
01396     /* Assign the number of errors if necessary */
01397     if (num_sick_errors)
01398     {
01399         *num_sick_errors = (unsigned int)num_errors;
01400     }
01401 
01402     /* Populate the return buffers with the error data */
01403     for (unsigned int i = 0, k = 1; i < (unsigned int)num_errors && (error_type_buffer || error_num_buffer); i++)
01404     {
01405 
01406         /* Check if the error type has been requested */
01407         if (error_type_buffer)
01408         {
01409             error_type_buffer[i] = payload_buffer[k];
01410         }
01411         k++;
01412 
01413         /* Check if the error number has been requested */
01414         if (error_num_buffer)
01415         {
01416             error_num_buffer[i] = payload_buffer[k];
01417         }
01418         k++;
01419 
01420     }
01421 
01422 }
01423 
01424 
01425 
01429 void SickPLS::_setSickOpModeInstallation( )
01430 throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException)
01431 {
01432 
01433     /* Assign the password for entering installation mode */
01434     uint8_t sick_password[9] = DEFAULT_SICK_PLS_SICK_PASSWORD;
01435 
01436     /* Check if mode should be changed */
01437     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_INSTALLATION)
01438     {
01439 
01440         try
01441         {
01442 
01443             /* Attempt to switch modes! */
01444             _switchSickOperatingMode(SICK_OP_MODE_INSTALLATION,sick_password);
01445 
01446         }
01447 
01448         /* Catch any config exceptions */
01449         catch(SickConfigException &sick_config_exception)
01450         {
01451             std::cerr << sick_config_exception.what() << std::endl;
01452             throw;
01453         }
01454 
01455         /* Catch any timeout exceptions */
01456         catch(SickTimeoutException &sick_timeout_exception)
01457         {
01458             std::cerr << sick_timeout_exception.what() << std::endl;
01459             throw;
01460         }
01461 
01462         /* Catch any I/O exceptions */
01463         catch(SickIOException &sick_io_exception)
01464         {
01465             std::cerr << sick_io_exception.what() << std::endl;
01466             throw;
01467         }
01468 
01469         /* Catch any thread exceptions */
01470         catch(SickThreadException &sick_thread_exception)
01471         {
01472             std::cerr << sick_thread_exception.what() << std::endl;
01473             throw;
01474         }
01475 
01476         /* Catch anything else */
01477         catch(...)
01478         {
01479             std::cerr << "SickPLS::_setSickOpModeInstallation: Unknown exception!!!" << std::endl;
01480             throw;
01481         }
01482 
01483         /* Assign the new operating mode */
01484         _sick_operating_status.sick_operating_mode = SICK_OP_MODE_INSTALLATION;
01485     }
01486 
01487 }
01488 
01492 void SickPLS::_setSickOpModeDiagnostic( )
01493 throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException)
01494 {
01495 
01496     /* Check if mode should be changed */
01497     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_DIAGNOSTIC)
01498     {
01499 
01500         std::cout << "\tAttempting to enter diagnostic mode..." << std::endl;
01501 
01502         try
01503         {
01504 
01505             /* Attempt to switch modes! */
01506             _switchSickOperatingMode(SICK_OP_MODE_DIAGNOSTIC);
01507 
01508         }
01509 
01510         /* Catch any config exceptions */
01511         catch(SickConfigException &sick_config_exception)
01512         {
01513             std::cerr << sick_config_exception.what() << std::endl;
01514             throw;
01515         }
01516 
01517         /* Catch any timeout exceptions */
01518         catch(SickTimeoutException &sick_timeout_exception)
01519         {
01520             std::cerr << sick_timeout_exception.what() << std::endl;
01521             throw;
01522         }
01523 
01524         /* Catch any I/O exceptions */
01525         catch(SickIOException &sick_io_exception)
01526         {
01527             std::cerr << sick_io_exception.what() << std::endl;
01528             throw;
01529         }
01530 
01531         /* Catch any thread exceptions */
01532         catch(SickThreadException &sick_thread_exception)
01533         {
01534             std::cerr << sick_thread_exception.what() << std::endl;
01535             throw;
01536         }
01537 
01538         /* Catch anything else */
01539         catch(...)
01540         {
01541             std::cerr << "SickPLS::_setSickOpModeInstallation: Unknown exception!!!" << std::endl;
01542             throw;
01543         }
01544 
01545         /* Assign the new operating mode */
01546         _sick_operating_status.sick_operating_mode = SICK_OP_MODE_DIAGNOSTIC;
01547 
01548         std::cout << "Success!" << std::endl;
01549 
01550     }
01551 
01552 }
01553 
01557 void SickPLS::_setSickOpModeMonitorRequestValues( )
01558 throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException)
01559 {
01560 
01561     /* Check if mode should be changed */
01562     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_REQUEST_VALUES)
01563     {
01564 
01565         try
01566         {
01567 
01568             /* Attempt to switch operating mode */
01569             _switchSickOperatingMode(SICK_OP_MODE_MONITOR_REQUEST_VALUES);
01570 
01571         }
01572 
01573         /* Catch any config exceptions */
01574         catch(SickConfigException &sick_config_exception)
01575         {
01576             std::cerr << sick_config_exception.what() << std::endl;
01577             throw;
01578         }
01579 
01580         /* Catch any timeout exceptions */
01581         catch(SickTimeoutException &sick_timeout_exception)
01582         {
01583             std::cerr << sick_timeout_exception.what() << std::endl;
01584             throw;
01585         }
01586 
01587         /* Catch any I/O exceptions */
01588         catch(SickIOException &sick_io_exception)
01589         {
01590             std::cerr << sick_io_exception.what() << std::endl;
01591             throw;
01592         }
01593 
01594         /* Catch any thread exceptions */
01595         catch(SickThreadException &sick_thread_exception)
01596         {
01597             std::cerr << sick_thread_exception.what() << std::endl;
01598             throw;
01599         }
01600 
01601         /* Catch anything else */
01602         catch(...)
01603         {
01604             std::cerr << "SickPLS::_setSickOpModeMonitorRequestValues: Unknown exception!!!" << std::endl;
01605             throw;
01606         }
01607 
01608         /* Assign the new operating mode */
01609         _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_REQUEST_VALUES;
01610 
01611     }
01612 
01613 }
01614 
01618 void SickPLS::_setSickOpModeMonitorStreamValues( )
01619 throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException)
01620 {
01621 
01622     /* Check if mode should be changed */
01623     if (_sick_operating_status.sick_operating_mode != SICK_OP_MODE_MONITOR_STREAM_VALUES)
01624     {
01625 
01626         std::cout << "\tRequesting measured value data stream..." << std::endl;
01627 
01628         try
01629         {
01630 
01631             /* Attempt to switch modes */
01632             _switchSickOperatingMode(SICK_OP_MODE_MONITOR_STREAM_VALUES);
01633 
01634         }
01635 
01636         /* Catch any config exceptions */
01637         catch(SickConfigException &sick_config_exception)
01638         {
01639             std::cerr << sick_config_exception.what() << std::endl;
01640             throw;
01641         }
01642 
01643         /* Catch any timeout exceptions */
01644         catch(SickTimeoutException &sick_timeout_exception)
01645         {
01646             std::cerr << sick_timeout_exception.what() << std::endl;
01647             throw;
01648         }
01649 
01650         /* Catch any I/O exceptions */
01651         catch(SickIOException &sick_io_exception)
01652         {
01653             std::cerr << sick_io_exception.what() << std::endl;
01654             throw;
01655         }
01656 
01657         /* Catch any thread exceptions */
01658         catch(SickThreadException &sick_thread_exception)
01659         {
01660             std::cerr << sick_thread_exception.what() << std::endl;
01661             throw;
01662         }
01663 
01664         /* Catch anything else */
01665         catch(...)
01666         {
01667             std::cerr << "SickPLS::_setSickOpModeMonitorStreamValues: Unknown exception!!!" << std::endl;
01668             throw;
01669         }
01670 
01671         /* Assign the new operating mode */
01672         _sick_operating_status.sick_operating_mode = SICK_OP_MODE_MONITOR_STREAM_VALUES;
01673 
01674         std::cout << "\t\tData stream started!" << std::endl;
01675 
01676     }
01677 
01678 }
01679 
01680 
01686 void SickPLS::_switchSickOperatingMode( const uint8_t sick_mode, const uint8_t * const mode_params )
01687 throw( SickConfigException, SickIOException, SickThreadException, SickTimeoutException)
01688 {
01689 
01690     SickPLSMessage message,response;
01691 
01692     uint8_t payload_buffer[SickPLSMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
01693     uint16_t num_partial_scans = 0;
01694 
01695     /* Construct the correct switch mode packet */
01696     payload_buffer[0] = 0x20;
01697     payload_buffer[1] = sick_mode;
01698 
01699     switch(sick_mode)
01700     {
01701 
01702     case SICK_OP_MODE_INSTALLATION:
01703 
01704         /* Make sure the params are defined */
01705         if(mode_params == NULL)
01706         {
01707             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01708         }
01709 
01710         memcpy(&payload_buffer[2],mode_params,8); //Copy password
01711         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,10);
01712         break;
01713 
01714     case SICK_OP_MODE_DIAGNOSTIC:
01715         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01716         break;
01717 
01718     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUE_FOR_EACH_SEGMENT:
01719         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01720         break;
01721 
01722     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VALUE_ON_OBJECT:
01723         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01724         break;
01725 
01726     case SICK_OP_MODE_MONITOR_STREAM_MIN_VERT_DIST_TO_OBJECT:
01727         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01728         break;
01729 
01730     case SICK_OP_MODE_MONITOR_TRIGGER_MIN_VERT_DIST_TO_OBJECT:
01731         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01732         break;
01733 
01734     case SICK_OP_MODE_MONITOR_STREAM_VALUES:
01735         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01736         break;
01737 
01738     case SICK_OP_MODE_MONITOR_REQUEST_VALUES:
01739         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01740         break;
01741 
01742     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES:
01743 
01744         /* Make sure the params are defined */
01745         if(mode_params == NULL)
01746         {
01747             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01748         }
01749 
01750         payload_buffer[2] = *mode_params;
01751         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,3);
01752         break;
01753 
01754     case SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE:
01755 
01756         /* Make sure the params are defined */
01757         if(mode_params == NULL)
01758         {
01759             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01760         }
01761 
01762         memcpy(&payload_buffer[2],mode_params,2);       //Begin range
01763         memcpy(&payload_buffer[4],&mode_params[2],2);   //End range
01764         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,6);
01765         break;
01766 
01767     case SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE:
01768 
01769         /* Make sure the params are defined */
01770         if(mode_params == NULL)
01771         {
01772             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01773         }
01774 
01775         payload_buffer[2] = mode_params[0];             //Sample size
01776         memcpy(&payload_buffer[3],&mode_params[1],2);   //Begin mean range
01777         memcpy(&payload_buffer[5],&mode_params[3],2);   //End mean range
01778         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,7);
01779         break;
01780 
01781     case SICK_OP_MODE_MONITOR_STREAM_VALUES_WITH_FIELDS:
01782 
01783         /* Make sure the params are defined */
01784         if(mode_params == NULL)
01785         {
01786             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01787         }
01788 
01789         memcpy(&payload_buffer[2],mode_params,2);       //Start
01790         memcpy(&payload_buffer[4],&mode_params[2],2);   //End
01791         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,6);
01792         break;
01793 
01794     case SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN:
01795         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01796         break;
01797 
01798     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT_FROM_PARTIAL_SCAN:
01799 
01800         /* Make sure the params are defined */
01801         if(mode_params == NULL)
01802         {
01803             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01804         }
01805 
01806         /* Get the number of partial scans (between 1 and 5) */
01807         memcpy(&num_partial_scans,mode_params,2);
01808 
01809         /* Setup the command packet */
01810         memcpy(&payload_buffer[2],mode_params,num_partial_scans*4+2);
01811         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,num_partial_scans*4+4);
01812         break;
01813 
01814     case SICK_OP_MODE_MONITOR_STREAM_MIN_VALUES_FOR_EACH_SEGMENT_SUBRANGE:
01815 
01816         /* Make sure the params are defined */
01817         if(mode_params == NULL)
01818         {
01819             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01820         }
01821 
01822         /* Get the number of partial scans (between 1 and 5) */
01823         memcpy(&num_partial_scans,mode_params,2);
01824 
01825         /* Setup the command packet */
01826         memcpy(&payload_buffer[2],mode_params,num_partial_scans*4+2);
01827         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,num_partial_scans*4+4);
01828         break;
01829 
01830     case SICK_OP_MODE_MONITOR_NAVIGATION:
01831         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,2);
01832         break;
01833 
01834     case SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT:
01835 
01836         /* Make sure the params are defined */
01837         if(mode_params == NULL)
01838         {
01839             throw SickConfigException("SickPLS::_switchSickOperatingMode - Requested mode requires parameters!");
01840         }
01841 
01842         memcpy(&payload_buffer[2],mode_params,2);       //Start
01843         memcpy(&payload_buffer[4],&mode_params[2],2);   //End
01844         message.BuildMessage(DEFAULT_SICK_PLS_SICK_ADDRESS,payload_buffer,6);
01845         break;
01846 
01847     case SICK_OP_MODE_UNKNOWN:
01848     //Let this case go straight to default
01849 
01850     default:
01851         throw SickConfigException("SickPLS::_switchSickOperatingMode: Unrecognized operating mode!");
01852     }
01853 
01854     //    message.Print();
01855 
01856     try
01857     {
01858 
01859         /* Attempt to send the message and get the reply */
01860         _sendMessageAndGetReply(message,
01861                                 response,
01862                                 DEFAULT_SICK_PLS_SICK_SWITCH_MODE_TIMEOUT,
01863                                 DEFAULT_SICK_PLS_NUM_TRIES);
01864 
01865     }
01866 
01867     /* Catch any timeout exceptions */
01868     catch(SickTimeoutException &sick_timeout_exception)
01869     {
01870         std::cerr << sick_timeout_exception.what() << std::endl;
01871         throw;
01872     }
01873 
01874     /* Catch any I/O exceptions */
01875     catch(SickIOException &sick_io_exception)
01876     {
01877         std::cerr << sick_io_exception.what() << std::endl;
01878         throw;
01879     }
01880 
01881     /* Catch any thread exceptions */
01882     catch(SickThreadException &sick_thread_exception)
01883     {
01884         std::cerr << sick_thread_exception.what() << std::endl;
01885         throw;
01886     }
01887 
01888     /* Catch anything else */
01889     catch(...)
01890     {
01891         std::cerr << "SickPLS::_switchSickOperatingMode: Unknown exception!!!" << std::endl;
01892         throw;
01893     }
01894 
01895     /* Reset the buffer */
01896     memset(payload_buffer,0,sizeof(payload_buffer));
01897 
01898     /* Obtain the response payload */
01899     response.GetPayload(payload_buffer);
01900 
01901     /* Make sure the reply was expected */
01902     if(payload_buffer[1] != 0x00)
01903     {
01904         throw SickConfigException("SickPLS::_switchSickOperatingMode: configuration request failed!");
01905     }
01906 
01907 }
01908 
01914 void SickPLS::_parseSickScanProfileB0( const uint8_t * const src_buffer, sick_pls_scan_profile_b0_t &sick_scan_profile ) const
01915 {
01916 
01917     /* Read block A, the number of measurments */
01918     sick_scan_profile.sick_num_measurements = src_buffer[0] + 256*(src_buffer[1] & 0x03);
01919 
01920     /* Check whether this is a partial scan */
01921     sick_scan_profile.sick_partial_scan_index = ((src_buffer[1] & 0x18) >> 3);
01922 
01923     /* Extract the measurements and Field values (if there are any) */
01924     _extractSickMeasurementValues(&src_buffer[2],
01925                                   sick_scan_profile.sick_num_measurements,
01926                                   sick_scan_profile.sick_measurements);
01927 
01928     /* If the Sick is pulling real-time indices then pull them too */
01929 
01930     // /* Buffer the Sick telegram index */
01931     // sick_scan_profile.sick_telegram_index = src_buffer[data_offset];
01932 
01933 }
01934 
01935 
01936 
01946 void SickPLS::_extractSickMeasurementValues( const uint8_t * const byte_sequence,
01947         const uint16_t num_measurements,
01948         uint16_t * const measured_values) const
01949 {
01950     /* Extract the range and Field values */
01951     for(unsigned int i = 0; i < num_measurements; i++)
01952     {
01953         measured_values[i] = byte_sequence[i*2] + 256*(byte_sequence[i*2+1] & 0x1F);
01954     }
01955 }
01956 
01961 bool SickPLS::_validSickScanAngle( const sick_pls_scan_angle_t sick_scan_angle ) const
01962 {
01963 
01964     /* Check the given Sick scan angle */
01965     if (sick_scan_angle != SICK_SCAN_ANGLE_180 )
01966     {
01967 
01968         return false;
01969     }
01970 
01971     /* Valid */
01972     return true;
01973 }
01974 
01979 bool SickPLS::_validSickScanResolution( const sick_pls_scan_resolution_t sick_scan_resolution ) const
01980 {
01981 
01982     /* Check the given Sick scan resolution value */
01983     if (sick_scan_resolution != SICK_SCAN_RESOLUTION_50)
01984     {
01985         return false;
01986     }
01987 
01988     /* Valid */
01989     return true;
01990 }
01991 
01992 
01993 
01994 
02000 sick_pls_baud_t SickPLS::_baudToSickBaud( const int baud_rate ) const
02001 {
02002 
02003     switch(baud_rate)
02004     {
02005     case B9600:
02006         return SICK_BAUD_9600;
02007     case B19200:
02008         return SICK_BAUD_19200;
02009     case B38400:
02010         return SICK_BAUD_38400;
02011     case B500000:
02012         return SICK_BAUD_500K;
02013     default:
02014         std::cerr << "Unexpected baud rate!" << std::endl;
02015         return SICK_BAUD_9600;
02016     }
02017 
02018 }
02019 
02020 
02021 
02022 } //namespace SickToolbox


asr_mild_base_laserscanner
Author(s): Aumann Florian, Borella Jocelyn, Dehmani Souheil, Marek Felix, Reckling Reno
autogenerated on Thu Jun 6 2019 21:02:16