SickPLS.hh

Go to the documentation of this file.

#ifndef SICK_PLS_HH
#define SICK_PLS_HH

/* Implementation dependencies */
#include <string>
#include <iostream>
#include <termios.h>

#include "SickLIDAR.hh"
#include "SickException.hh"

#include "SickPLSBufferMonitor.hh"
#include "SickPLSMessage.hh"

/* Macro definitions */
#define DEFAULT_SICK_PLS_SICK_BAUD                                       (B500000)  ///< Initial baud rate of the PLS (whatever is set in flash)
#define DEFAULT_SICK_PLS_HOST_ADDRESS                                     (0x80)  ///< Client/host default serial address
#define DEFAULT_SICK_PLS_SICK_ADDRESS                                     (0x00)  ///< Sick PLS default serial address
#define DEFAULT_SICK_PLS_SICK_PASSWORD                                "SICK_PLS"  ///< Password for entering installation mode
#define DEFAULT_SICK_PLS_SICK_MESSAGE_TIMEOUT                (unsigned int)(20e6)  ///< The max time to wait for a message reply (usecs)
#define DEFAULT_SICK_PLS_SICK_SWITCH_MODE_TIMEOUT            (unsigned int)(20e6)  ///< Can take the Sick LD up to 3 seconds to reply (usecs)
#define DEFAULT_SICK_PLS_SICK_CONFIG_MESSAGE_TIMEOUT        (unsigned int)(20e6)  ///< The sick can take some time to respond to config commands (usecs)
#define DEFAULT_SICK_PLS_BYTE_INTERVAL                                      (55)  ///< Minimum time in microseconds between transmitted bytes
//#define DEFAULT_SICK_PLS_BYTE_INTERVAL                                      (0)  ///< Minimum time in microseconds between transmitted bytes
#define DEFAULT_SICK_PLS_NUM_TRIES                                           (1)  ///< The max number of tries before giving up on a request

/* Associate the namespace */
namespace SickToolbox {

  class SickPLS : public SickLIDAR< SickPLSBufferMonitor, SickPLSMessage >
  {

  public:

        /* TODO: I think this should be 180 * 2 */
    static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 721;                     


    enum sick_pls_scan_angle_t {
      SICK_SCAN_ANGLE_180 = 180,                                               
      SICK_SCAN_ANGLE_UNKNOWN = 0xFF                                           
    };

    enum sick_pls_scan_resolution_t {
      SICK_SCAN_RESOLUTION_50 = 50,                                            
      SICK_SCAN_RESOLUTION_UNKNOWN = 0xFF                                      
    };

    enum sick_pls_measuring_units_t {

      /* Only in cm */
      SICK_MEASURING_UNITS_CM = 0x00,                                          
      SICK_MEASURING_UNITS_UNKNOWN = 0xFF                                      
    };

    enum sick_pls_status_t {
      SICK_STATUS_OK = 0x00,                                                   
      SICK_STATUS_ERROR = 0x01,                                                
      SICK_STATUS_UNKNOWN = 0xFF                                               
    };

    enum sick_pls_operating_mode_t {

      SICK_OP_MODE_INSTALLATION = 0x00,                                        
      SICK_OP_MODE_DIAGNOSTIC = 0x10,                                          
      SICK_OP_MODE_MONITOR_STREAM_MIN_VALUE_FOR_EACH_SEGMENT = 0x20,           
      SICK_OP_MODE_MONITOR_TRIGGER_MIN_VALUE_ON_OBJECT = 0x21,                 
      SICK_OP_MODE_MONITOR_STREAM_MIN_VERT_DIST_TO_OBJECT = 0x22,              
      SICK_OP_MODE_MONITOR_TRIGGER_MIN_VERT_DIST_TO_OBJECT = 0x23,             
      SICK_OP_MODE_MONITOR_STREAM_VALUES = 0x24,                               
      SICK_OP_MODE_MONITOR_REQUEST_VALUES = 0x25,                              
      SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES = 0x26,                          
      SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE = 0x27,                      
      SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE = 0x28,                 
      SICK_OP_MODE_MONITOR_STREAM_VALUES_WITH_FIELDS = 0x29,                   
      SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN = 0x2A,             
      SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT_FROM_PARTIAL_SCAN = 0x2B,  
      SICK_OP_MODE_MONITOR_STREAM_MIN_VALUES_FOR_EACH_SEGMENT_SUBRANGE = 0x2C, 
      SICK_OP_MODE_MONITOR_NAVIGATION = 0x2E,                                  
      SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT = 0x50,                    
      SICK_OP_MODE_UNKNOWN = 0xFF                                              
    };

    enum sick_pls_baud_t {
      SICK_BAUD_9600 = 0x42,                                                   
      SICK_BAUD_19200 = 0x41,                                                  
      SICK_BAUD_38400 = 0x40,                                                  
      SICK_BAUD_500K = 0x48,                                                   
      SICK_BAUD_UNKNOWN = 0xFF                                                 
    };


    typedef struct sick_pls_operating_status_tag {
      uint16_t sick_scan_angle;                                                
      uint16_t sick_scan_resolution;                                           
      uint16_t sick_num_motor_revs;                                            
      uint8_t sick_operating_mode;                                             
      uint8_t sick_laser_mode;                                                 
      uint8_t sick_measuring_units;                                            
      uint8_t sick_address;                                                    
    } sick_pls_operating_status_t;



    typedef struct sick_pls_baud_status_tag {
      uint16_t sick_baud_rate;                                                 
      uint8_t sick_permanent_baud_rate;                                        
    } sick_pls_baud_status_t;


    typedef struct sick_pls_scan_profile_b0_tag {
      uint16_t sick_num_measurements;                                          
      uint16_t sick_measurements[SICK_MAX_NUM_MEASUREMENTS];                   
      uint8_t sick_telegram_index;                                             
      uint8_t sick_real_time_scan_index;                                       
      uint8_t sick_partial_scan_index;                                         
    } sick_pls_scan_profile_b0_t;


    SickPLS( const std::string sick_device_path );

    ~SickPLS( );

    bool Initialize( const sick_pls_baud_t desired_baud_rate )
      throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    void Uninitialize( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );

    std::string GetSickDevicePath( ) const;

    double GetSickScanAngle( ) const throw( SickConfigException );

    double GetSickScanResolution( ) const throw( SickConfigException );

    SickPLS::sick_pls_measuring_units_t GetSickMeasuringUnits( ) const throw( SickConfigException );

    sick_pls_operating_mode_t GetSickOperatingMode( ) const throw( SickConfigException );

    void GetSickScan( unsigned int * const measurement_values,
                      unsigned int & num_measurement_values) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    sick_pls_status_t GetSickStatus( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );

    void ResetSick( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );

    std::string GetSickStatusAsString( ) const;




    /*
     * NOTE: The following methods are given to make working with our
     *       predefined types a bit more manageable.
     */


    static sick_pls_scan_angle_t IntToSickScanAngle( const int scan_angle_int );

    static sick_pls_scan_resolution_t IntToSickScanResolution( const int scan_resolution_int );

    static sick_pls_scan_resolution_t DoubleToSickScanResolution( const double scan_resolution_double );

    static std::string SickBaudToString( const sick_pls_baud_t baud_rate );

    static sick_pls_baud_t IntToSickBaud( const int baud_int );

    static sick_pls_baud_t StringToSickBaud( const std::string baud_str );

    static std::string SickStatusToString( const sick_pls_status_t sick_status );

    static std::string SickOperatingModeToString( const sick_pls_operating_mode_t sick_operating_mode );

    static std::string SickMeasuringUnitsToString( const sick_pls_measuring_units_t sick_units );

  protected:

    std::string _sick_device_path;

    sick_pls_baud_t _curr_session_baud;

    sick_pls_baud_t _desired_session_baud;


  sick_pls_operating_status_t _sick_operating_status;


    sick_pls_baud_status_t _sick_baud_status;


    struct termios _old_term;

    void _setupConnection( ) throw( SickIOException, SickThreadException );

    void _teardownConnection( ) throw( SickIOException );

    void _sendMessageAndGetReply( const SickPLSMessage &sick_send_message,
                                  SickPLSMessage &sick_recv_message,
                                  const unsigned int timeout_value,
                                  const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException );

    void _sendMessageAndGetReply( const SickPLSMessage &sick_send_message,
                                  SickPLSMessage &sick_recv_message,
                                  const uint8_t reply_code,
                                  const unsigned int timeout_value,
                                  const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException );

    void _flushTerminalBuffer( ) throw ( SickThreadException );

    void _setSessionBaud( const sick_pls_baud_t baud_rate ) throw( SickIOException, SickThreadException, SickTimeoutException );

    bool _testSickBaud( const sick_pls_baud_t baud_rate ) throw( SickIOException, SickThreadException );

    void _setTerminalBaud( const sick_pls_baud_t sick_baud ) throw( SickIOException, SickThreadException );

    void _getSickErrors( unsigned int * const num_sick_errors = NULL,
                         uint8_t * const error_type_buffer = NULL,
                         uint8_t * const error_num_buffer = NULL ) throw( SickTimeoutException, SickIOException, SickThreadException );

    void _setSickOpModeInstallation( )
      throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    void _setSickOpModeDiagnostic( )
      throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    void _setSickOpModeMonitorRequestValues( )
      throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    void _setSickOpModeMonitorStreamValues( )
      throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    void _switchSickOperatingMode( const uint8_t sick_mode, const uint8_t * const mode_params = NULL )
      throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);

    void _parseSickScanProfileB0( const uint8_t * const src_buffer, sick_pls_scan_profile_b0_t &sick_scan_profile ) const;


    void _extractSickMeasurementValues( const uint8_t * const byte_sequence, const uint16_t num_measurements, uint16_t * const measured_values) const;

    bool _validSickScanAngle( const sick_pls_scan_angle_t sick_scan_angle ) const;

    bool _validSickScanResolution( const sick_pls_scan_resolution_t sick_scan_resolution ) const;

    sick_pls_baud_t _baudToSickBaud( const int baud_rate ) const;

  };

  typedef SickPLS::sick_pls_scan_angle_t sick_pls_scan_angle_t;

  typedef SickPLS::sick_pls_scan_resolution_t sick_pls_scan_resolution_t;

  typedef SickPLS::sick_pls_measuring_units_t sick_pls_measuring_units_t;


  typedef SickPLS::sick_pls_operating_mode_t sick_pls_operating_mode_t;

  typedef SickPLS::sick_pls_baud_t sick_pls_baud_t;

} //namespace SickToolbox

#endif //SICK_PLS_HH