SickLD.hh

Go to the documentation of this file.

#ifndef SICK_LD_HH
#define SICK_LD_HH

/* Macros */
#define DEFAULT_SICK_IP_ADDRESS                          "192.168.1.10"  ///< Default Sick LD INet 4 address
#define DEFAULT_SICK_TCP_PORT                                   (49152)  ///< Default TCP port
#define DEFAULT_SICK_MESSAGE_TIMEOUT                (unsigned int)(5e6)  ///< The max time to wait for a message reply (usecs)
#define DEFAULT_SICK_CONNECT_TIMEOUT                (unsigned int)(1e6)  ///< The max time to wait before considering a connection attempt as failed (usecs)
#define DEFAULT_SICK_NUM_SCAN_PROFILES                              (0)  ///< Setting this value to 0 will tell the Sick LD to stream measurements when measurement data is requested (NOTE: A profile is a single scans worth of range measurements)
#define DEFAULT_SICK_SIGNAL_SET                                     (0)  ///< Default Sick signal configuration

#define SWAP_VALUES(x,y,t) (t=x,x=y,y=t);

/* Definition dependencies */
#include <string>
#include <vector>
#include <pthread.h>
#include <arpa/inet.h>

#include "SickLIDAR.hh"
#include "SickLDBufferMonitor.hh"
#include "SickLDMessage.hh"
#include "SickException.hh"

namespace SickToolbox {

  class SickLD : public SickLIDAR< SickLDBufferMonitor, SickLDMessage > {

  public:

    /* Some constants for the developer/end-user */
    static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 2881;                             
    static const uint16_t SICK_MAX_NUM_SECTORS = 8;                                     
    static const uint16_t SICK_MAX_NUM_MEASURING_SECTORS = 4;                           
    static const uint16_t SICK_MAX_SCAN_AREA = 360;                                     
    static const uint16_t SICK_MIN_MOTOR_SPEED = 5;                                     
    static const uint16_t SICK_MAX_MOTOR_SPEED = 20;                                    
    static const uint16_t SICK_MIN_VALID_SENSOR_ID = 1;                                 
    static const uint16_t SICK_MAX_VALID_SENSOR_ID = 254;                               
    static const uint16_t SICK_MAX_MEAN_PULSE_FREQUENCY = 10800;                        
    static const uint16_t SICK_MAX_PULSE_FREQUENCY = 14400;                             
    static const uint16_t SICK_NUM_TICKS_PER_MOTOR_REV = 5760;                          
    static const double SICK_MAX_SCAN_ANGULAR_RESOLUTION = 0.125;                       
    static const double SICK_DEGREES_PER_MOTOR_STEP = 0.0625;                           
    
    /* Sick LD sensor modes of operation */
    static const uint8_t SICK_SENSOR_MODE_IDLE = 0x01;                                  
    static const uint8_t SICK_SENSOR_MODE_ROTATE = 0x02;                                
    static const uint8_t SICK_SENSOR_MODE_MEASURE = 0x03;                               
    static const uint8_t SICK_SENSOR_MODE_ERROR = 0x04;                                 
    static const uint8_t SICK_SENSOR_MODE_UNKNOWN = 0xFF;                               
  
    /* Sick LD motor modes */
    static const uint8_t SICK_MOTOR_MODE_OK = 0x00;                                     
    static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_HIGH = 0x09;                          
    static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_LOW = 0x04;                           
    static const uint8_t SICK_MOTOR_MODE_ERROR = 0x0B;                                  
    static const uint8_t SICK_MOTOR_MODE_UNKNOWN = 0xFF;                                
  
    /* Sick LD service codes */
    static const uint8_t SICK_STAT_SERV_CODE = 0x01;                                    
    static const uint8_t SICK_CONF_SERV_CODE = 0x02;                                    
    static const uint8_t SICK_MEAS_SERV_CODE = 0x03;                                    
    static const uint8_t SICK_WORK_SERV_CODE = 0x04;                                    
    static const uint8_t SICK_ROUT_SERV_CODE = 0x06;                                    
    static const uint8_t SICK_FILE_SERV_CODE = 0x07;                                    
    static const uint8_t SICK_MONR_SERV_CODE = 0x08;                                    
  
    /* Sick LD status services (service code 0x01) */
    static const uint8_t SICK_STAT_SERV_GET_ID = 0x01;                                  
    static const uint8_t SICK_STAT_SERV_GET_STATUS = 0x02;                              
    static const uint8_t SICK_STAT_SERV_GET_SIGNAL = 0x04;                              
    static const uint8_t SICK_STAT_SERV_SET_SIGNAL = 0x05;                              
    static const uint8_t SICK_STAT_SERV_LD_REGISTER_APPLICATION = 0x06;                 

    /* Sick LD status service GET_IDENTIFICATION request codes */
    static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM = 0x00;                  
    static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_NAME = 0x01;                      
    static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_VERSION = 0x02;                   
    static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM = 0x03;                
    static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM = 0x04;            
    static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM = 0x10;                
    static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_NAME = 0x11;                    
    static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION = 0x12;                 
    static const uint8_t SICK_STAT_SERV_GET_ID_APP_PART_NUM = 0x20;                     
    static const uint8_t SICK_STAT_SERV_GET_ID_APP_NAME = 0x21;                         
    static const uint8_t SICK_STAT_SERV_GET_ID_APP_VERSION = 0x22;                      
  
    /* Sick LD configuration services (service code 0x02) */
    static const uint8_t SICK_CONF_SERV_SET_CONFIGURATION = 0x01;                       
    static const uint8_t SICK_CONF_SERV_GET_CONFIGURATION = 0x02;                       
    static const uint8_t SICK_CONF_SERV_SET_TIME_ABSOLUTE = 0x03;                       
    static const uint8_t SICK_CONF_SERV_SET_TIME_RELATIVE = 0x04;                       
    static const uint8_t SICK_CONF_SERV_GET_SYNC_CLOCK = 0x05;                          
    static const uint8_t SICK_CONF_SERV_SET_FILTER = 0x09;                              
    static const uint8_t SICK_CONF_SERV_SET_FUNCTION = 0x0A;                            
    static const uint8_t SICK_CONF_SERV_GET_FUNCTION = 0x0B;                            

    /* Sick LD configuration filter codes */
    static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD = 0x01;                    
  
    /* Sick LD nearfield suppression configuration codes */
    static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF = 0x00;                
    static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON = 0x01;                 
  
    /* Sick LD measurement services (service code 0x03) */
    static const uint8_t SICK_MEAS_SERV_GET_PROFILE = 0x01;                             
    static const uint8_t SICK_MEAS_SERV_CANCEL_PROFILE = 0x02;                          

    /* Sick LD working services (service code 0x04) */
    static const uint8_t SICK_WORK_SERV_RESET = 0x01;                                   
    static const uint8_t SICK_WORK_SERV_TRANS_IDLE = 0x02;                              
    static const uint8_t SICK_WORK_SERV_TRANS_ROTATE = 0x03;                            
    static const uint8_t SICK_WORK_SERV_TRANS_MEASURE = 0x04;                           

    /* Sick LD working service DO_RESET request codes */
    static const uint8_t SICK_WORK_SERV_RESET_INIT_CPU = 0x00;                          
    static const uint8_t SICK_WORK_SERV_RESET_KEEP_CPU = 0x01;                          
    static const uint8_t SICK_WORK_SERV_RESET_HALT_APP = 0x02;                          

    /* Sick LD working service TRANS_MEASURE return codes */
    static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_OK = 0x00;                    
    static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE = 0x01;         
    static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE = 0x02;        
    static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER = 0x03;       
    static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT = 0x04;  
  
    /* Sick LD interface routing services (service code 0x06) */
    static const uint8_t SICK_ROUT_SERV_COM_ATTACH = 0x01;                              
    static const uint8_t SICK_ROUT_SERV_COM_DETACH = 0x02;                              
    static const uint8_t SICK_ROUT_SERV_COM_INITIALIZE = 0x03;                          
    static const uint8_t SICK_ROUT_SERV_COM_OUTPUT = 0x04;                              
    static const uint8_t SICK_ROUT_SERV_COM_DATA = 0x05;                                

    /* Sick LD file services (service code 0x07) */
    static const uint8_t SICK_FILE_SERV_DIR = 0x01;                                     
    static const uint8_t SICK_FILE_SERV_SAVE = 0x02;                                    
    static const uint8_t SICK_FILE_SERV_LOAD = 0x03;                                    
    static const uint8_t SICK_FILE_SERV_DELETE = 0x04;                                  

    /* Sick LD monitor services (service code 0x08) */
    static const uint8_t SICK_MONR_SERV_MONITOR_RUN = 0x01;                             
    static const uint8_t SICK_MONR_SERV_MONITOR_PROFILE_LOG = 0x02;                     

    /* Sick LD configuration keys */
    static const uint8_t SICK_CONF_KEY_RS232_RS422 = 0x01;                              
    static const uint8_t SICK_CONF_KEY_CAN = 0x02;                                      
    static const uint8_t SICK_CONF_KEY_ETHERNET = 0x05;                                 
    static const uint8_t SICK_CONF_KEY_GLOBAL = 0x10;                                   

    /* Sick LD sector configuration codes */
    static const uint8_t SICK_CONF_SECTOR_NOT_INITIALIZED = 0x00;                       
    static const uint8_t SICK_CONF_SECTOR_NO_MEASUREMENT = 0x01;                        
    static const uint8_t SICK_CONF_SECTOR_RESERVED = 0x02;                              
    static const uint8_t SICK_CONF_SECTOR_NORMAL_MEASUREMENT = 0x03;                    
    static const uint8_t SICK_CONF_SECTOR_REFERENCE_MEASUREMENT = 0x04;                 

    /* Sick LD profile formats */
    static const uint16_t SICK_SCAN_PROFILE_RANGE = 0x39FF;                             
    /*
     * SICK_SCAN_PROFILE_RANGE format (0x39FF) interpretation: 
     * (See page 32 of telegram listing for fieldname definitions)
     *
     * Field Name   | Send
     * --------------------
     * PROFILESENT  | YES
     * PROFILECOUNT | YES
     * LAYERNUM     | YES
     * SECTORNUM    | YES
     * DIRSTEP      | YES
     * POINTNUM     | YES
     * TSTART       | YES
     * STARTDIR     | YES
     * DISTANCE-n   | YES
     * DIRECTION-n  | NO
     * ECHO-n       | NO
     * TEND         | YES
     * ENDDIR       | YES
     * SENSTAT      | YES
     */

    /* Sick LD profile formats */
    static const uint16_t SICK_SCAN_PROFILE_RANGE_AND_ECHO = 0x3DFF;                    
    /*
     * SICK_SCAN_PROFILE_RANGE format (0x3DFF) interpretation: 
     * (See page 32 of telegram listing for fieldname definitions)
     *
     * Field Name   | Send
     * --------------------
     * PROFILESENT  | YES
     * PROFILECOUNT | YES
     * LAYERNUM     | YES
     * SECTORNUM    | YES
     * DIRSTEP      | YES
     * POINTNUM     | YES
     * TSTART       | YES
     * STARTDIR     | YES
     * DISTANCE-n   | YES
     * DIRECTION-n  | NO
     * ECHO-n       | YES
     * TEND         | YES
     * ENDDIR       | YES
     * SENSTAT      | YES
     */
  
    /* Masks for working with the Sick LD signals
     *
     * NOTE: Although the Sick LD manual defines the flag
     *       values for red and green LEDs the operation
     *       of these LEDs are reserved. So they can't
     *       be set by the device driver.
     */
    static const uint8_t SICK_SIGNAL_LED_YELLOW_A = 0x01;                               
    static const uint8_t SICK_SIGNAL_LED_YELLOW_B = 0x02;                               
    static const uint8_t SICK_SIGNAL_LED_GREEN = 0x04;                                  
    static const uint8_t SICK_SIGNAL_LED_RED = 0x08;                                    
    static const uint8_t SICK_SIGNAL_SWITCH_0 = 0x10;                                   
    static const uint8_t SICK_SIGNAL_SWITCH_1 = 0x20;                                   
    static const uint8_t SICK_SIGNAL_SWITCH_2 = 0x40;                                   
    static const uint8_t SICK_SIGNAL_SWITCH_3 = 0x80;                                   

    typedef struct sick_ld_config_global_tag {
      uint16_t sick_sensor_id;                                                            
      uint16_t sick_motor_speed;                                                          
      double sick_angle_step;                                                             
    } sick_ld_config_global_t;
    
    typedef struct sick_ld_config_ethernet_tag {
      uint16_t sick_ip_address[4];                                                        
      uint16_t sick_subnet_mask[4];                                                       
      uint16_t sick_gateway_ip_address[4];                                                
      uint16_t sick_node_id;                                                              
      uint16_t sick_transparent_tcp_port;                                                 
    } sick_ld_config_ethernet_t;
    
    typedef struct sick_ld_config_sector_tag {
      uint8_t sick_num_active_sectors;                                                    
      uint8_t sick_num_initialized_sectors;                                               
      uint8_t sick_active_sector_ids[SICK_MAX_NUM_SECTORS];                               
      uint8_t sick_sector_functions[SICK_MAX_NUM_SECTORS];                                
      double sick_sector_start_angles[SICK_MAX_NUM_SECTORS];                              
      double sick_sector_stop_angles[SICK_MAX_NUM_SECTORS];                               
    } sick_ld_config_sector_t;
    
    typedef struct sick_ld_identity_tag {
      std::string sick_part_number;                                                       
      std::string sick_name;                                                              
      std::string sick_version;                                                           
      std::string sick_serial_number;                                                     
      std::string sick_edm_serial_number;                                                 
      std::string sick_firmware_part_number;                                              
      std::string sick_firmware_name;                                                     
      std::string sick_firmware_version;                                                  
      std::string sick_application_software_part_number;                                  
      std::string sick_application_software_name;                                         
      std::string sick_application_software_version;                                      
    } sick_ld_identity_t;
    
    typedef struct sick_ld_sector_data_tag {
      unsigned int sector_num;                                                            
      unsigned int num_data_points;                                                       
      unsigned int timestamp_start;                                                       
      unsigned int timestamp_stop;                                                        
      unsigned int echo_values[SICK_MAX_NUM_MEASUREMENTS];                                
      double angle_step;                                                                  
      double angle_start;                                                                 
      double angle_stop;                                                                  
      double range_values[SICK_MAX_NUM_MEASUREMENTS];                                     
      double scan_angles[SICK_MAX_NUM_MEASUREMENTS];                                      
    } sick_ld_sector_data_t;
    
    typedef struct sick_ld_scan_profile_tag {
      unsigned int profile_number;                                                        
      unsigned int profile_counter;                                                       
      unsigned int layer_num;                                                             
      unsigned int sensor_status;                                                         
      unsigned int motor_status;                                                          
      unsigned int num_sectors;                                                           
      sick_ld_sector_data_t sector_data[SICK_MAX_NUM_SECTORS];                            
    } sick_ld_scan_profile_t;
    
    SickLD( const std::string sick_ip_address = DEFAULT_SICK_IP_ADDRESS,
            const uint16_t sick_tcp_port = DEFAULT_SICK_TCP_PORT );
    
    void Initialize( )  throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );

    void GetSickStatus( unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode )
      throw( SickIOException, SickTimeoutException );

    void SetSickTempScanAreas( const double * active_sector_start_angles, const double * const active_sector_stop_angles,
                               const unsigned int num_active_sectors )
      throw( SickTimeoutException, SickIOException, SickConfigException );
    
    void SetSickTimeAbsolute( const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time )
      throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );

    void SetSickTimeRelative( const int16_t time_delta, uint16_t &new_sick_clock_time )
      throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );

    void GetSickTime( uint16_t &sick_time )
      throw( SickIOException, SickTimeoutException, SickErrorException );
  
    void SetSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
      throw( SickIOException, SickTimeoutException, SickErrorException );

    void GetSickSignals( uint8_t &sick_signal_flags ) throw( SickIOException, SickTimeoutException );
  
    void EnableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void DisableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void GetSickMeasurements( double * const range_measurements,
                              unsigned int * const echo_measurements = NULL,
                              unsigned int * const num_measurements = NULL,
                              unsigned int * const sector_ids = NULL,
                              unsigned int * const sector_data_offsets = NULL,
                              double * const sector_step_angles = NULL,
                              double * const sector_start_angles = NULL,
                              double * const sector_stop_angles = NULL,
                              unsigned int * const sector_start_timestamps = NULL,
                              unsigned int * const sector_stop_timestamps = NULL )
      throw( SickErrorException, SickIOException, SickTimeoutException, SickConfigException );

    void SetSickSensorID( const unsigned int sick_sensor_id )
      throw( SickErrorException, SickTimeoutException, SickIOException );

    void SetSickMotorSpeed( const unsigned int sick_motor_speed )
      throw( SickErrorException, SickTimeoutException, SickIOException );

    void SetSickScanResolution( const double sick_step_angle )
      throw( SickTimeoutException, SickIOException, SickConfigException );

    void SetSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed,
                                          const double sick_step_angle,
                                          const double * const active_sector_start_angles,
                                          const double * const active_sector_stop_angles,
                                          const unsigned int num_active_sectors )
      throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );

    void SetSickScanAreas( const double * const active_sector_start_angles,
                           const double * const active_sector_stop_angles,
                           const unsigned int num_active_sectors )
      throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );

    void ResetSick( const unsigned int reset_level = SICK_WORK_SERV_RESET_INIT_CPU )
      throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );

    unsigned int GetSickNumActiveSectors( ) const;
  
    unsigned int GetSickSensorID( ) const;

    unsigned int GetSickMotorSpeed( ) const;

    double GetSickScanResolution( ) const;
  
    std::string GetSickIPAddress( ) const;
  
    std::string GetSickSubnetMask( ) const;
  
    std::string GetSickGatewayIPAddress( ) const;
  
    std::string GetSickPartNumber( ) const;

    std::string GetSickName( ) const;

    std::string GetSickVersion( ) const;

    std::string GetSickSerialNumber( ) const;

    std::string GetSickEDMSerialNumber( ) const;

    std::string GetSickFirmwarePartNumber( ) const;

    std::string GetSickFirmwareName( ) const;

    std::string GetSickFirmwareVersion( ) const;

    std::string GetSickAppSoftwarePartNumber( ) const;

    std::string GetSickAppSoftwareName( ) const;

    std::string GetSickAppSoftwareVersionNumber( ) const;

    std::string GetSickStatusAsString() const;

    std::string GetSickIdentityAsString() const;

    std::string GetSickGlobalConfigAsString() const;

    std::string GetSickEthernetConfigAsString() const;

    std::string GetSickSectorConfigAsString() const;
    
    double GetSickScanArea( ) const;
  
    void PrintSickStatus( ) const;

    void PrintSickIdentity( ) const;

    void PrintSickGlobalConfig( ) const;

    void PrintSickEthernetConfig( ) const;

    void PrintSickSectorConfig( ) const;
  
    void Uninitialize( ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );

    ~SickLD();

  private:

    std::string _sick_ip_address;

    uint16_t _sick_tcp_port;

    unsigned int _socket;
  
    struct sockaddr_in _sick_inet_address_info;

    uint8_t _sick_sensor_mode;

    uint8_t _sick_motor_mode;

    bool _sick_streaming_range_data;

    bool _sick_streaming_range_and_echo_data;
  
    sick_ld_identity_t _sick_identity;

    sick_ld_config_global_t _sick_global_config;
  
    sick_ld_config_ethernet_t _sick_ethernet_config;

    sick_ld_config_sector_t _sick_sector_config;

    void _setupConnection( ) throw( SickIOException, SickTimeoutException );
  
    void _syncDriverWithSick( ) throw( SickIOException, SickTimeoutException, SickErrorException );

    void _setSickSectorFunction( const uint8_t sector_number, const uint8_t sector_function,
                                 const double sector_angle_stop, const bool write_to_flash = false )
       throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
  
    void _getSickSectorFunction( const uint8_t sector_num, uint8_t &sector_function, double &sector_stop_angle )
      throw( SickErrorException, SickTimeoutException, SickIOException );
  
    void _setSickSensorModeToIdle( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void _setSickSensorModeToRotate( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void _setSickSensorModeToMeasure( ) throw( SickErrorException, SickTimeoutException, SickIOException );
  
    void _setSickSensorMode( const uint8_t new_sick_sensor_mode )
      throw( SickErrorException, SickTimeoutException, SickIOException );
  
    void _getSickScanProfiles( const uint16_t profile_format, const uint16_t num_profiles = DEFAULT_SICK_NUM_SCAN_PROFILES )
      throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );

    void _parseScanProfile( uint8_t * const src_buffer, sick_ld_scan_profile_t &profile_data ) const;

    void _cancelSickScanProfiles( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void _setSickFilter( const uint8_t suppress_code )
      throw( SickErrorException, SickTimeoutException, SickIOException );
  
    void _getSickIdentity( ) throw( SickTimeoutException, SickIOException );

    void _getSickStatus( ) throw( SickTimeoutException, SickIOException );

    void _setSickGlobalConfig( const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step )
      throw( SickErrorException, SickTimeoutException, SickIOException );

    void _getSickGlobalConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void _getSickEthernetConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );

    void _getSickSectorConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
  
    void _getIdentificationString( const uint8_t id_request_code, std::string &id_return_string )
      throw( SickTimeoutException, SickIOException );
  
    void _getSensorPartNumber( ) throw( SickTimeoutException, SickIOException );

    void _getSensorName( ) throw( SickTimeoutException, SickIOException );

    void _getSensorVersion( ) throw( SickTimeoutException, SickIOException );

    void _getSensorSerialNumber( ) throw( SickTimeoutException, SickIOException );

    void _getSensorEDMSerialNumber( ) throw( SickTimeoutException, SickIOException );

    void _getFirmwarePartNumber( ) throw( SickTimeoutException, SickIOException );

    void _getFirmwareName( ) throw( SickTimeoutException, SickIOException );

    void _getFirmwareVersion( ) throw( SickTimeoutException, SickIOException );

    void _getApplicationSoftwarePartNumber( ) throw( SickTimeoutException, SickIOException );

    void _getApplicationSoftwareName( ) throw( SickTimeoutException, SickIOException );

    void _getApplicationSoftwareVersion( ) throw( SickTimeoutException, SickIOException );

    void _setSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed, const double sick_step_angle,
                                           const double * const active_sector_start_angles,
                                           const double * const active_sector_stop_angles,
                                           const unsigned int num_active_sectors )
       throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );

    void _setSickTemporaryScanAreas( const double * const active_sector_start_angles,
                                     const double * const active_sector_stop_angles,
                                     const unsigned int num_active_sectors )
      throw( SickTimeoutException, SickIOException, SickConfigException );

    void _setSickSectorConfig( const unsigned int * const sector_functions, const double * const sector_stop_angles,
                               const unsigned int num_sectors, const bool write_to_flash = false )
      throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );

    void _setSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
      throw( SickIOException, SickTimeoutException, SickErrorException );
  
    void _sendMessageAndGetReply( const SickLDMessage &send_message, SickLDMessage &recv_message,
                                  const unsigned int timeout_value = DEFAULT_SICK_MESSAGE_TIMEOUT ) 
      throw( SickIOException, SickTimeoutException );

    void _flushTCPRecvBuffer( ) throw ( SickIOException, SickThreadException );
    
    void _teardownConnection( ) throw( SickIOException );

    void _generateSickSectorConfig( const double * const active_sector_start_angles,
                                    const double * const active_sector_stop_angles,
                                    const unsigned int num_active_sectors,
                                    const double sick_step_angle,
                                    unsigned int * const sector_functions, 
                                    double * const sector_stop_angles,
                                    unsigned int &num_sectors ) const;
  
    double _ticksToAngle( const uint16_t ticks ) const;

    uint16_t _angleToTicks( const double angle ) const;

    double _computeMeanPulseFrequency( const double active_scan_area, const double curr_motor_speed,
                                       const double curr_angular_resolution ) const;
  
    double _computeMaxPulseFrequency( const double total_scan_area, const double curr_motor_speed,
                                      const double curr_angular_resolution ) const;

    bool _validSickSensorID( const unsigned int sick_sensor_id ) const;

    bool _validSickMotorSpeed( const unsigned int sick_motor_speed ) const;

    bool _validSickScanResolution( const double sick_step_angle, const double * const active_sector_start_angles,
                                   const double * const active_sector_stop_angles, const unsigned int num_active_sectors ) const;

    bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle ) const;
  
    bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle,
                               const double * const active_sector_start_angles,
                               const double * const active_sector_stop_angles,
                               const unsigned int num_active_sectors ) const;
  
    double _computeScanArea( const double sick_step_angle, const double * const sector_start_angles,
                             const double * const sector_stop_angles, const unsigned int num_sectors ) const;

    void _sortScanAreas( double * const sector_start_angles, double * const sector_stop_angles,
                         const unsigned int num_sectors ) const;

    bool _validActiveSectors( const double * const sector_start_angles, const double * const sector_stop_angles,
                              const unsigned int num_active_sectors ) const;
    
    bool _supportedScanProfileFormat( const uint16_t profile_format ) const; 

    void _printSectorProfileData( const sick_ld_sector_data_t &sector_data ) const;

    void _printSickScanProfile( const sick_ld_scan_profile_t profile_data, const bool print_sector_data = true ) const;

    uint8_t _sickSensorModeToWorkServiceSubcode( const uint8_t sick_sensor_mode ) const;

    std::string _sickSensorModeToString( const uint8_t sick_sensor_mode ) const;
 
    std::string _sickMotorModeToString( const uint8_t sick_motor_mode ) const;

    std::string _sickTransMeasureReturnToString( const uint8_t return_value ) const;
  
    std::string _sickResetLevelToString( const uint16_t reset_level ) const;
  
    std::string _sickSectorFunctionToString( const uint16_t sick_sector_function ) const;
  
    std::string _sickProfileFormatToString( const uint16_t profile_format ) const;

    void _printInitFooter( ) const;

  };

} //namespace SickToolbox
  
#endif /* SICK_LD_HH */