c++/drivers/ld/sickld-1.0/SickLD.hh

Go to the documentation of this file.
00001 /*!
00002  * \file SickLD.hh
00003  * \brief Defines the SickLD class for working with the
00004  *        Sick LD-OEM/LD-LRS long range LIDARs.
00005  *
00006  * Code by Jason C. Derenick and Thomas H. Miller.
00007  * Contact derenick(at)lehigh(dot)edu
00008  *
00009  * The Sick LIDAR Matlab/C++ Toolbox
00010  * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
00011  * All rights reserved.
00012  *
00013  * This software is released under a BSD Open-Source License.
00014  * See http://sicktoolbox.sourceforge.net
00015  */
00016 
00017 #ifndef SICK_LD_HH
00018 #define SICK_LD_HH
00019 
00020 /* Macros */
00021 #define DEFAULT_SICK_IP_ADDRESS                          "192.168.1.10"  ///< Default Sick LD INet 4 address
00022 #define DEFAULT_SICK_TCP_PORT                                   (49152)  ///< Default TCP port
00023 #define DEFAULT_SICK_MESSAGE_TIMEOUT                (unsigned int)(5e6)  ///< The max time to wait for a message reply (usecs)
00024 #define DEFAULT_SICK_CONNECT_TIMEOUT                (unsigned int)(1e6)  ///< The max time to wait before considering a connection attempt as failed (usecs)
00025 #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)
00026 #define DEFAULT_SICK_SIGNAL_SET                                     (0)  ///< Default Sick signal configuration
00027 
00028 /**
00029  * \def SWAP_VALUES(x,y,t)
00030  * \brief A simple macro for swapping two values.
00031  */
00032 #define SWAP_VALUES(x,y,t) (t=x,x=y,y=t);
00033 
00034 /* Auto-generated header */
00035 #include "SickConfig.hh"
00036 
00037 /* Definition dependencies */
00038 #include <string>
00039 #include <vector>
00040 #include <pthread.h>
00041 #include <arpa/inet.h>
00042 
00043 #include "SickLIDAR.hh"
00044 #include "SickLDBufferMonitor.hh"
00045 #include "SickLDMessage.hh"
00046 #include "SickException.hh"
00047 
00048 /**
00049  * \namespace SickToolbox
00050  * \brief Encapsulates the Sick LIDAR Matlab/C++ toolbox
00051  */
00052 namespace SickToolbox {
00053 
00054   /**
00055    * \class SickLD
00056    * \brief Provides a simple driver interface for working with the
00057    *        Sick LD-OEM/Sick LD-LRS long-range models via Ethernet.
00058    */
00059   class SickLD : public SickLIDAR< SickLDBufferMonitor, SickLDMessage > {
00060 
00061   public:
00062 
00063     /* Some constants for the developer/end-user */
00064     static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 2881;                             ///< Maximum number of measurements per sector
00065     static const uint16_t SICK_MAX_NUM_SECTORS = 8;                                     ///< Maximum number of scan sectors (NOTE: This value must be even)
00066     static const uint16_t SICK_MAX_NUM_MEASURING_SECTORS = 4;                           ///< Maximum number of active/measuring scan sectors
00067     static const uint16_t SICK_MAX_SCAN_AREA = 360;                                     ///< Maximum area that can be covered in a single scan (deg)
00068     static const uint16_t SICK_MIN_MOTOR_SPEED = 5;                                     ///< Minimum motor speed in Hz
00069     static const uint16_t SICK_MAX_MOTOR_SPEED = 20;                                    ///< Maximum motor speed in Hz
00070     static const uint16_t SICK_MIN_VALID_SENSOR_ID = 1;                                 ///< The lowest value the Sick will accept as a Sensor ID
00071     static const uint16_t SICK_MAX_VALID_SENSOR_ID = 254;                               ///< The largest value the Sick will accept as a Sensor ID    
00072     static const uint16_t SICK_MAX_MEAN_PULSE_FREQUENCY = 10800;                        ///< Max mean pulse frequence of the current device configuration (in Hz) (see page 22 of the operator's manual)
00073     static const uint16_t SICK_MAX_PULSE_FREQUENCY = 14400;                             ///< Max pulse frequency of the device (in Hz) (see page 22 of the operator's manual)
00074     static const uint16_t SICK_NUM_TICKS_PER_MOTOR_REV = 5760;                          ///< Odometer ticks per revolution of the Sick LD scan head
00075     static const double SICK_MAX_SCAN_ANGULAR_RESOLUTION = 0.125;                       ///< Minimum valid separation between laser pulses in active scan ares (deg)
00076     static const double SICK_DEGREES_PER_MOTOR_STEP = 0.0625;                           ///< Each odometer tick is equivalent to rotating the scan head this many degrees
00077     
00078     /* Sick LD sensor modes of operation */
00079     static const uint8_t SICK_SENSOR_MODE_IDLE = 0x01;                                  ///< The Sick LD is powered but idle
00080     static const uint8_t SICK_SENSOR_MODE_ROTATE = 0x02;                                ///< The Sick LD prism is rotating, but laser is off
00081     static const uint8_t SICK_SENSOR_MODE_MEASURE = 0x03;                               ///< The Sick LD prism is rotating, and the laser is on
00082     static const uint8_t SICK_SENSOR_MODE_ERROR = 0x04;                                 ///< The Sick LD is in error mode
00083     static const uint8_t SICK_SENSOR_MODE_UNKNOWN = 0xFF;                               ///< The Sick LD is in an unknown state
00084   
00085     /* Sick LD motor modes */
00086     static const uint8_t SICK_MOTOR_MODE_OK = 0x00;                                     ///< Motor is functioning properly
00087     static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_HIGH = 0x09;                          ///< Motor spin too low (i.e. rotational velocity too low)
00088     static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_LOW = 0x04;                           ///< Motor spin too high (i.e. rotational velocity too fast)
00089     static const uint8_t SICK_MOTOR_MODE_ERROR = 0x0B;                                  ///< Motor stops or coder error
00090     static const uint8_t SICK_MOTOR_MODE_UNKNOWN = 0xFF;                                ///< Motor is in an unknown state
00091   
00092     /* Sick LD service codes */
00093     static const uint8_t SICK_STAT_SERV_CODE = 0x01;                                    ///< Status service code 
00094     static const uint8_t SICK_CONF_SERV_CODE = 0x02;                                    ///< Configuration service code
00095     static const uint8_t SICK_MEAS_SERV_CODE = 0x03;                                    ///< Measurement service code
00096     static const uint8_t SICK_WORK_SERV_CODE = 0x04;                                    ///< Working service code
00097     static const uint8_t SICK_ROUT_SERV_CODE = 0x06;                                    ///< Routing service code
00098     static const uint8_t SICK_FILE_SERV_CODE = 0x07;                                    ///< File service code 
00099     static const uint8_t SICK_MONR_SERV_CODE = 0x08;                                    ///< Monitor service code 
00100   
00101     /* Sick LD status services (service code 0x01) */
00102     static const uint8_t SICK_STAT_SERV_GET_ID = 0x01;                                  ///< Request the Sick LD ID
00103     static const uint8_t SICK_STAT_SERV_GET_STATUS = 0x02;                              ///< Request status information
00104     static const uint8_t SICK_STAT_SERV_GET_SIGNAL = 0x04;                              ///< Reads the value of the switch and LED port
00105     static const uint8_t SICK_STAT_SERV_SET_SIGNAL = 0x05;                              ///< Sets the switches and LEDs
00106     static const uint8_t SICK_STAT_SERV_LD_REGISTER_APPLICATION = 0x06;                 ///< Registers the ID data for the application firmware
00107 
00108     /* Sick LD status service GET_IDENTIFICATION request codes */
00109     static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM = 0x00;                  ///< Request the sensor's part number
00110     static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_NAME = 0x01;                      ///< Request the sensor's name
00111     static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_VERSION = 0x02;                   ///< Request the sensor's version
00112     static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM = 0x03;                ///< Request the sensor's serial number
00113     static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM = 0x04;            ///< Request the edm??? serial number
00114     static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM = 0x10;                ///< Requess the firmware's part number
00115     static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_NAME = 0x11;                    ///< Request the firmware's name
00116     static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION = 0x12;                 ///< Request the firmware's version
00117     static const uint8_t SICK_STAT_SERV_GET_ID_APP_PART_NUM = 0x20;                     ///< Request the application part number
00118     static const uint8_t SICK_STAT_SERV_GET_ID_APP_NAME = 0x21;                         ///< Request the application name
00119     static const uint8_t SICK_STAT_SERV_GET_ID_APP_VERSION = 0x22;                      ///< Request the application version
00120   
00121     /* Sick LD configuration services (service code 0x02) */
00122     static const uint8_t SICK_CONF_SERV_SET_CONFIGURATION = 0x01;                       ///< Set the Sick LD configuration
00123     static const uint8_t SICK_CONF_SERV_GET_CONFIGURATION = 0x02;                       ///< Read the Sick LD configuration information
00124     static const uint8_t SICK_CONF_SERV_SET_TIME_ABSOLUTE = 0x03;                       ///< Set the internal clock to a timestamp value
00125     static const uint8_t SICK_CONF_SERV_SET_TIME_RELATIVE = 0x04;                       ///< Correct the internal clock by some value
00126     static const uint8_t SICK_CONF_SERV_GET_SYNC_CLOCK = 0x05;                          ///< Read the internal time of the LD-OEM/LD-LRS
00127     static const uint8_t SICK_CONF_SERV_SET_FILTER = 0x09;                              ///< Set the filter configuration
00128     static const uint8_t SICK_CONF_SERV_SET_FUNCTION = 0x0A;                            ///< Assigns a measurement function to an angle range
00129     static const uint8_t SICK_CONF_SERV_GET_FUNCTION = 0x0B;                            ///< Returns the configuration of the given sector
00130 
00131     /* Sick LD configuration filter codes */
00132     static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD = 0x01;                    ///< Code for identifying filter type: nearfield suppression
00133   
00134     /* Sick LD nearfield suppression configuration codes */
00135     static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF = 0x00;                ///< Used to set nearfield suppression off
00136     static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON = 0x01;                 ///< Used to set nearfield suppression on
00137   
00138     /* Sick LD measurement services (service code 0x03) */
00139     static const uint8_t SICK_MEAS_SERV_GET_PROFILE = 0x01;                             ///< Requests n profiles of a defined format
00140     static const uint8_t SICK_MEAS_SERV_CANCEL_PROFILE = 0x02;                          ///< Stops profile output
00141 
00142     /* Sick LD working services (service code 0x04) */
00143     static const uint8_t SICK_WORK_SERV_RESET = 0x01;                                   ///< Sick LD enters a reset sequence
00144     static const uint8_t SICK_WORK_SERV_TRANS_IDLE = 0x02;                              ///< Sick LD enters IDLE mode (motor stops and laser is turned off)
00145     static const uint8_t SICK_WORK_SERV_TRANS_ROTATE = 0x03;                            ///< Sick LD enters ROTATE mode (motor starts and rotates with a specified speed in Hz, laser is off)
00146     static const uint8_t SICK_WORK_SERV_TRANS_MEASURE = 0x04;                           ///< Sick LD enters MEASURE mode (laser starts with next revolution) 
00147 
00148     /* Sick LD working service DO_RESET request codes */
00149     static const uint8_t SICK_WORK_SERV_RESET_INIT_CPU = 0x00;                          ///< Sick LD does a complete reset (Reinitializes the CPU)
00150     static const uint8_t SICK_WORK_SERV_RESET_KEEP_CPU = 0x01;                          ///< Sick LD does a partial reset (CPU is not reinitialized)
00151     static const uint8_t SICK_WORK_SERV_RESET_HALT_APP = 0x02;                          ///< Sick LD does a minimal reset (Application is halted and device enters IDLE state)
00152 
00153     /* Sick LD working service TRANS_MEASURE return codes */
00154     static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_OK = 0x00;                    ///< Sick LD is ready to stream/obtain scan profiles
00155     static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE = 0x01;         ///< Sick LD reports config yields a max laser pulse frequency that is too high
00156     static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE = 0x02;        ///< Sick LD reports config yields a max mean pulse frequency that is too high
00157     static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER = 0x03;       ///< Sick LD reports sector borders are not configured correctly
00158     static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT = 0x04;  ///< Sick LD reports sector borders are not a multiple of the step angle
00159   
00160     /* Sick LD interface routing services (service code 0x06) */
00161     static const uint8_t SICK_ROUT_SERV_COM_ATTACH = 0x01;                              ///< Attach a master (host) communications interface
00162     static const uint8_t SICK_ROUT_SERV_COM_DETACH = 0x02;                              ///< Detach a master (host) communications interface
00163     static const uint8_t SICK_ROUT_SERV_COM_INITIALIZE = 0x03;                          ///< Initialize the interface (Note: using this may not be necessary for some interfaces, e.g. Ethernet) 
00164     static const uint8_t SICK_ROUT_SERV_COM_OUTPUT = 0x04;                              ///< Output data to the interface
00165     static const uint8_t SICK_ROUT_SERV_COM_DATA = 0x05;                                ///< Forward data received on specified interface to master interface
00166 
00167     /* Sick LD file services (service code 0x07) */
00168     static const uint8_t SICK_FILE_SERV_DIR = 0x01;                                     ///< List the stored files in flash memory
00169     static const uint8_t SICK_FILE_SERV_SAVE = 0x02;                                    ///< Saves the data into flash memory
00170     static const uint8_t SICK_FILE_SERV_LOAD = 0x03;                                    ///< Recalls a file from the flash
00171     static const uint8_t SICK_FILE_SERV_DELETE = 0x04;                                  ///< Deletes a file from the flash
00172 
00173     /* Sick LD monitor services (service code 0x08) */
00174     static const uint8_t SICK_MONR_SERV_MONITOR_RUN = 0x01;                             ///< Enable/disable monitor services
00175     static const uint8_t SICK_MONR_SERV_MONITOR_PROFILE_LOG = 0x02;                     ///< Enable/disable profile logging
00176 
00177     /* Sick LD configuration keys */
00178     static const uint8_t SICK_CONF_KEY_RS232_RS422 = 0x01;                              ///< Key for configuring RS-232/RS-422
00179     static const uint8_t SICK_CONF_KEY_CAN = 0x02;                                      ///< Key for configuring CAN 
00180     static const uint8_t SICK_CONF_KEY_ETHERNET = 0x05;                                 ///< Key for configuring Ethernet
00181     static const uint8_t SICK_CONF_KEY_GLOBAL = 0x10;                                   ///< Key for global configuration
00182 
00183     /* Sick LD sector configuration codes */
00184     static const uint8_t SICK_CONF_SECTOR_NOT_INITIALIZED = 0x00;                       ///< Sector is uninitialized
00185     static const uint8_t SICK_CONF_SECTOR_NO_MEASUREMENT = 0x01;                        ///< Sector has no measurements
00186     static const uint8_t SICK_CONF_SECTOR_RESERVED = 0x02;                              ///< Sector is reserved by Sick LD 
00187     static const uint8_t SICK_CONF_SECTOR_NORMAL_MEASUREMENT = 0x03;                    ///< Sector is returning measurements
00188     static const uint8_t SICK_CONF_SECTOR_REFERENCE_MEASUREMENT = 0x04;                 ///< Sector can be used as reference measurement
00189 
00190     /* Sick LD profile formats */
00191     static const uint16_t SICK_SCAN_PROFILE_RANGE = 0x39FF;                             ///< Request sector scan data w/o any echo data
00192     /*
00193      * SICK_SCAN_PROFILE_RANGE format (0x39FF) interpretation: 
00194      * (See page 32 of telegram listing for fieldname definitions)
00195      *
00196      * Field Name   | Send
00197      * --------------------
00198      * PROFILESENT  | YES
00199      * PROFILECOUNT | YES
00200      * LAYERNUM     | YES
00201      * SECTORNUM    | YES
00202      * DIRSTEP      | YES
00203      * POINTNUM     | YES
00204      * TSTART       | YES
00205      * STARTDIR     | YES
00206      * DISTANCE-n   | YES
00207      * DIRECTION-n  | NO
00208      * ECHO-n       | NO
00209      * TEND         | YES
00210      * ENDDIR       | YES
00211      * SENSTAT      | YES
00212      */
00213 
00214     /* Sick LD profile formats */
00215     static const uint16_t SICK_SCAN_PROFILE_RANGE_AND_ECHO = 0x3DFF;                    ///< Request sector scan data w/ echo data
00216     /*
00217      * SICK_SCAN_PROFILE_RANGE format (0x3DFF) interpretation: 
00218      * (See page 32 of telegram listing for fieldname definitions)
00219      *
00220      * Field Name   | Send
00221      * --------------------
00222      * PROFILESENT  | YES
00223      * PROFILECOUNT | YES
00224      * LAYERNUM     | YES
00225      * SECTORNUM    | YES
00226      * DIRSTEP      | YES
00227      * POINTNUM     | YES
00228      * TSTART       | YES
00229      * STARTDIR     | YES
00230      * DISTANCE-n   | YES
00231      * DIRECTION-n  | NO
00232      * ECHO-n       | YES
00233      * TEND         | YES
00234      * ENDDIR       | YES
00235      * SENSTAT      | YES
00236      */
00237   
00238     /* Masks for working with the Sick LD signals
00239      *
00240      * NOTE: Although the Sick LD manual defines the flag
00241      *       values for red and green LEDs the operation
00242      *       of these LEDs are reserved. So they can't
00243      *       be set by the device driver.
00244      */
00245     static const uint8_t SICK_SIGNAL_LED_YELLOW_A = 0x01;                               ///< Mask for first yellow LED
00246     static const uint8_t SICK_SIGNAL_LED_YELLOW_B = 0x02;                               ///< Mask for second yellow LED
00247     static const uint8_t SICK_SIGNAL_LED_GREEN = 0x04;                                  ///< Mask for green LED
00248     static const uint8_t SICK_SIGNAL_LED_RED = 0x08;                                    ///< Mask for red LED
00249     static const uint8_t SICK_SIGNAL_SWITCH_0 = 0x10;                                   ///< Mask for signal switch 0
00250     static const uint8_t SICK_SIGNAL_SWITCH_1 = 0x20;                                   ///< Mask for signal switch 1
00251     static const uint8_t SICK_SIGNAL_SWITCH_2 = 0x40;                                   ///< Mask for signal switch 2
00252     static const uint8_t SICK_SIGNAL_SWITCH_3 = 0x80;                                   ///< Mask for signal switch 3
00253 
00254     /**
00255      * \struct sick_ld_config_global_tag
00256      * \brief A structure to aggregate the data used to configure the
00257      *        Sick LD global parameter values.
00258      */
00259     /**
00260      * \typedef sick_ld_config_global_t
00261      * \brief Adopt c-style convention
00262      */
00263     typedef struct sick_ld_config_global_tag {
00264       uint16_t sick_sensor_id;                                                            ///< The single word sensor ID for the Sick unit
00265       uint16_t sick_motor_speed;                                                          ///< Nominal motor speed value: 0x0005 to 0x0014 (5 to 20)
00266       double sick_angle_step;                                                             ///< Difference between two laser pulse positions in 1/16th deg. (NOTE: this value must be a divisor of 5760 and be greater than 1)  
00267     } sick_ld_config_global_t;
00268     
00269     /**
00270      * \struct sick_ld_config_ethernet_tag
00271      * \brief A structure to aggregate the data used to configure
00272      *        the Sick LD unit for Ethernet.
00273      *
00274      * \todo Eventually add similar config structures for the other protocols.
00275      */
00276     /**
00277      * \typedef sick_ld_config_ethernet_t
00278      * \brief Adopt c-style convention
00279      */
00280     typedef struct sick_ld_config_ethernet_tag {
00281       uint16_t sick_ip_address[4];                                                        ///< IP address in numerical form w/ leftmost part at sick_ip_address[0]
00282       uint16_t sick_subnet_mask[4];                                                       ///< Subnet mask for the network to which the Sick LD is assigned
00283       uint16_t sick_gateway_ip_address[4];                                                ///< The address of the local gateway
00284       uint16_t sick_node_id;                                                              ///< Single word address of the Sick LD
00285       uint16_t sick_transparent_tcp_port;                                                 ///< The TCP/IP transparent port associated with the Sick LD
00286     } sick_ld_config_ethernet_t;
00287     
00288     /**
00289      * \struct sick_ld_config_sector_tag
00290      * \brief A structure to aggregate data used to define the
00291      *        Sick LD's sector configuration.
00292      */
00293     /**
00294      * \typedef sick_ld_config_sector_t
00295      * \brief Adopt c-style convention
00296      */
00297     typedef struct sick_ld_config_sector_tag {
00298       uint8_t sick_num_active_sectors;                                                    ///< Number of active sectors (sectors that are actually being scanned)
00299       uint8_t sick_num_initialized_sectors;                                               ///< Number of sectors configured w/ a function other than "not initialized" 
00300       uint8_t sick_active_sector_ids[SICK_MAX_NUM_SECTORS];                               ///< IDs of all active sectors
00301       uint8_t sick_sector_functions[SICK_MAX_NUM_SECTORS];                                ///< Function values associated w/ each of the Sick LD's sectors
00302       double sick_sector_start_angles[SICK_MAX_NUM_SECTORS];                              ///< Start angles for each initialized sector (deg)
00303       double sick_sector_stop_angles[SICK_MAX_NUM_SECTORS];                               ///< Stop angles for each sector (deg)
00304     } sick_ld_config_sector_t;
00305     
00306     /**
00307      * \struct sick_ld_identity_tag
00308      * \brief A structure to aggregate the fields that collectively
00309      *        define the identity of a Sick LD unit.
00310      */
00311     /**
00312      * \typedef sick_ld_identity_t
00313      * \brief Adopt c-style convention
00314      */
00315     typedef struct sick_ld_identity_tag {
00316       std::string sick_part_number;                                                       ///< The Sick LD's part number
00317       std::string sick_name;                                                              ///< The name assigned to the Sick
00318       std::string sick_version;                                                           ///< The Sick LD's version number
00319       std::string sick_serial_number;                                                     ///< The Sick LD's serial number
00320       std::string sick_edm_serial_number;                                                 ///< The Sick LD's edm??? serial number
00321       std::string sick_firmware_part_number;                                              ///< The Sick LD's firmware part number 
00322       std::string sick_firmware_name;                                                     ///< The Sick LD's firmware name
00323       std::string sick_firmware_version;                                                  ///< The Sick LD's firmware version
00324       std::string sick_application_software_part_number;                                  ///< The Sick LD's app. software part number
00325       std::string sick_application_software_name;                                         ///< The Sick LD's app. software name
00326       std::string sick_application_software_version;                                      ///< The Sick LD's app. software version
00327     } sick_ld_identity_t;
00328     
00329     /**
00330      * \struct sick_ld_sector_data_tag
00331      * \brief A structure to aggregate the fields that collectively
00332      *        define a sector in the scan area of the Sick LD unit.
00333      */
00334     /**
00335      * \typedef sick_ld_sector_data_t
00336      * \brief Adopt c-style convention
00337      */
00338     typedef struct sick_ld_sector_data_tag {
00339       unsigned int sector_num;                                                            ///< The sector number in the scan area
00340       unsigned int num_data_points;                                                       ///< The number of data points in the scan area
00341       unsigned int timestamp_start;                                                       ///< The timestamp (in ms) corresponding to the time the first measurement in the sector was taken 
00342       unsigned int timestamp_stop;                                                        ///< The timestamp (in ms) corresponding to the time the last measurement in the sector was taken
00343       unsigned int echo_values[SICK_MAX_NUM_MEASUREMENTS];                                ///< The corresponding echo/reflectivity values
00344       double angle_step;                                                                  ///< The angle step used for the given sector (this should be the same for all sectors)
00345       double angle_start;                                                                 ///< The angle at which the first measurement in the sector was acquired
00346       double angle_stop;                                                                  ///< The angle at which the last measurement in the sector was acquired
00347       double range_values[SICK_MAX_NUM_MEASUREMENTS];                                     ///< The corresponding range values (NOTE: The size of this array is intended to be large enough to accomodate various sector configs.)
00348       double scan_angles[SICK_MAX_NUM_MEASUREMENTS];                                      ///< The scan angles corresponding to the respective measurements
00349     } sick_ld_sector_data_t;
00350     
00351     /**
00352      * \struct sick_ld_scan_profile_tag
00353      * \brief A structure to aggregate the fields that collectively
00354      *        define the profile of a single scan acquired from the
00355      *        Sick LD unit.
00356      */
00357     /**
00358      * \typedef sick_ld_scan_profile_t
00359      * \brief Adopt c-style convention
00360      */
00361     typedef struct sick_ld_scan_profile_tag {
00362       unsigned int profile_number;                                                        ///< The number of profiles sent to the host (i.e. the current profile number)
00363       unsigned int profile_counter;                                                       ///< The number of profiles gathered by the Sick LD
00364       unsigned int layer_num;                                                             ///< The layer number associated with a scan (this will always be 0)
00365       unsigned int sensor_status;                                                         ///< The status of the Sick LD sensor
00366       unsigned int motor_status;                                                          ///< The status of the Sick LD motor
00367       unsigned int num_sectors;                                                           ///< The number of sectors returned in the profile
00368       sick_ld_sector_data_t sector_data[SICK_MAX_NUM_SECTORS];                            ///< The sectors associated with the scan profile 
00369     } sick_ld_scan_profile_t;
00370     
00371     /** Primary constructor */
00372     SickLD( const std::string sick_ip_address = DEFAULT_SICK_IP_ADDRESS,
00373             const uint16_t sick_tcp_port = DEFAULT_SICK_TCP_PORT );
00374     
00375     /** Initializes the Sick LD unit (use scan areas defined in flash) */
00376     void Initialize( )  throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
00377 
00378     /** Gets the sensor and motor mode of the unit */
00379     void GetSickStatus( unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode )
00380       throw( SickIOException, SickTimeoutException );
00381 
00382     /** Sets the temporal scan configuration (until power is cycled) */
00383     void SetSickTempScanAreas( const double * active_sector_start_angles, const double * const active_sector_stop_angles,
00384                                const unsigned int num_active_sectors )
00385       throw( SickTimeoutException, SickIOException, SickConfigException );
00386     
00387     /** Sets the internal clock of the Sick LD unit */
00388     void SetSickTimeAbsolute( const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time )
00389       throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00390 
00391     /** Sets the internal clock of the Sick LD using the relative given time value */
00392     void SetSickTimeRelative( const int16_t time_delta, uint16_t &new_sick_clock_time )
00393       throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00394 
00395     /** Gets the internal clock time of the Sick LD unit */
00396     void GetSickTime( uint16_t &sick_time )
00397       throw( SickIOException, SickTimeoutException, SickErrorException );
00398   
00399     /** Sets the signal LEDs and switches */
00400     void SetSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
00401       throw( SickIOException, SickTimeoutException, SickErrorException );
00402 
00403     /** Query the Sick for its current signal settings */
00404     void GetSickSignals( uint8_t &sick_signal_flags ) throw( SickIOException, SickTimeoutException );
00405   
00406     /** Enables nearfield suppressive filtering (in flash) */
00407     void EnableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00408 
00409     /** Disables nearfield suppressive filtering (in flash) */
00410     void DisableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00411 
00412     /** Acquires measurements and related data for all active sectors */
00413     void GetSickMeasurements( double * const range_measurements,
00414                               unsigned int * const echo_measurements = NULL,
00415                               unsigned int * const num_measurements = NULL,
00416                               unsigned int * const sector_ids = NULL,
00417                               unsigned int * const sector_data_offsets = NULL,
00418                               double * const sector_step_angles = NULL,
00419                               double * const sector_start_angles = NULL,
00420                               double * const sector_stop_angles = NULL,
00421                               unsigned int * const sector_start_timestamps = NULL,
00422                               unsigned int * const sector_stop_timestamps = NULL )
00423       throw( SickErrorException, SickIOException, SickTimeoutException, SickConfigException );
00424 
00425     /** Attempts to set a new senor ID for the device (in flash) */
00426     void SetSickSensorID( const unsigned int sick_sensor_id )
00427       throw( SickErrorException, SickTimeoutException, SickIOException );
00428 
00429     /** Attempts to set a new motor speed for the device (in flash) */
00430     void SetSickMotorSpeed( const unsigned int sick_motor_speed )
00431       throw( SickErrorException, SickTimeoutException, SickIOException );
00432 
00433     /** Attempts to set a new scan resolution for the device (in flash) */
00434     void SetSickScanResolution( const double sick_step_angle )
00435       throw( SickTimeoutException, SickIOException, SickConfigException );
00436 
00437     /** Attempts to set the global params and the active scan sectors for the device (in flash) */
00438     void SetSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed,
00439                                           const double sick_step_angle,
00440                                           const double * const active_sector_start_angles,
00441                                           const double * const active_sector_stop_angles,
00442                                           const unsigned int num_active_sectors )
00443       throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
00444 
00445     /** Attempts to set the active scan sectors for the device (in flash) */
00446     void SetSickScanAreas( const double * const active_sector_start_angles,
00447                            const double * const active_sector_stop_angles,
00448                            const unsigned int num_active_sectors )
00449       throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
00450 
00451     /** Resets the Sick LD using the given reset level */
00452     void ResetSick( const unsigned int reset_level = SICK_WORK_SERV_RESET_INIT_CPU )
00453       throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00454 
00455     /** Returns the number of active/measuring sectors */
00456     unsigned int GetSickNumActiveSectors( ) const;
00457   
00458     /** Acquire the Sick LD's sensor ID */
00459     unsigned int GetSickSensorID( ) const;
00460 
00461     /** Acquire the Sick LD's current motor speed in Hz */
00462     unsigned int GetSickMotorSpeed( ) const;
00463 
00464     /** Acquire the Sick LD's current scan resolution */
00465     double GetSickScanResolution( ) const;
00466   
00467     /** Acquire the current IP address of the Sick */
00468     std::string GetSickIPAddress( ) const;
00469   
00470     /** Acquire the subnet mask for the Sick */
00471     std::string GetSickSubnetMask( ) const;
00472   
00473     /** Acquire the IP address of the Sick gateway */
00474     std::string GetSickGatewayIPAddress( ) const;
00475   
00476     /** Acquire the Sick LD's part number */
00477     std::string GetSickPartNumber( ) const;
00478 
00479     /** Acquire the Sick LD's name */
00480     std::string GetSickName( ) const;
00481 
00482     /** Acquire the Sick LD's version number */
00483     std::string GetSickVersion( ) const;
00484 
00485     /** Acquire the Sick LD's serial number */
00486     std::string GetSickSerialNumber( ) const;
00487 
00488     /** Acquire the Sick LD's EDM serial number */
00489     std::string GetSickEDMSerialNumber( ) const;
00490 
00491     /** Acquire the Sick LD's firmware part number */
00492     std::string GetSickFirmwarePartNumber( ) const;
00493 
00494     /** Acquire the Sick LD's firmware number */
00495     std::string GetSickFirmwareName( ) const;
00496 
00497     /** Acquire the Sick LD's firmware version */
00498     std::string GetSickFirmwareVersion( ) const;
00499 
00500     /** Acquire the Sick LD's application software part number */
00501     std::string GetSickAppSoftwarePartNumber( ) const;
00502 
00503     /** Acquire the Sick LD's application software name */
00504     std::string GetSickAppSoftwareName( ) const;
00505 
00506     /** Acquire the Sick LD's application software version number */
00507     std::string GetSickAppSoftwareVersionNumber( ) const;
00508 
00509     /** Acquire the Sick LD's status as a printable string */
00510     std::string GetSickStatusAsString() const;
00511 
00512     /** Acquire the Sick LD's identity as a printable string */
00513     std::string GetSickIdentityAsString() const;
00514 
00515     /** Acquire the Sick LD's global config as a printable string */
00516     std::string GetSickGlobalConfigAsString() const;
00517 
00518     /** Acquire the Sick LD's Ethernet config as a printable string */
00519     std::string GetSickEthernetConfigAsString() const;
00520 
00521     /** Acquire the Sick LD's sector config as a printable string */
00522     std::string GetSickSectorConfigAsString() const;
00523     
00524     /** Acquire the total scan area (in degrees) being scanned by the Sick LD */
00525     double GetSickScanArea( ) const;
00526   
00527     /** Prints the Sick LD's status information */
00528     void PrintSickStatus( ) const;
00529 
00530     /** Prints the Sick LD's identity information */
00531     void PrintSickIdentity( ) const;
00532 
00533     /** Prints the global configuration parameter values */
00534     void PrintSickGlobalConfig( ) const;
00535 
00536     /** Prints the Ethernet configuration parameter values */
00537     void PrintSickEthernetConfig( ) const;
00538 
00539     /** Prints the Sick Sector configuration */
00540     void PrintSickSectorConfig( ) const;
00541   
00542     /** Uninitializes the Sick LD unit */
00543     void Uninitialize( ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
00544 
00545     /** Destructor */
00546     ~SickLD();
00547 
00548   private:
00549 
00550     /** The Sick LD IP address */
00551     std::string _sick_ip_address;
00552 
00553     /** The Sick LD TCP port number */
00554     uint16_t _sick_tcp_port;
00555 
00556     /** Sick LD socket structure */
00557     unsigned int _socket;
00558   
00559     /** Sick LD socket address structure */
00560     struct sockaddr_in _sick_inet_address_info;
00561 
00562     /** The current sensor mode */
00563     uint8_t _sick_sensor_mode;
00564 
00565     /** The mode of the motor */
00566     uint8_t _sick_motor_mode;
00567 
00568     /** Indicates whether the Sick LD is currently streaming range data */
00569     bool _sick_streaming_range_data;
00570 
00571     /** Indicates whether the Sick LD is currently streaming range and echo data */
00572     bool _sick_streaming_range_and_echo_data;
00573   
00574     /** The identity structure for the Sick */
00575     sick_ld_identity_t _sick_identity;
00576 
00577     /** The current global configuration for the unit */
00578     sick_ld_config_global_t _sick_global_config;
00579   
00580     /** The current Ethernet configuration for the unit */
00581     sick_ld_config_ethernet_t _sick_ethernet_config;
00582 
00583     /** The current sector configuration for the unit */
00584     sick_ld_config_sector_t _sick_sector_config;
00585 
00586     /** Setup the connection parameters and establish TCP connection! */
00587     void _setupConnection( ) throw( SickIOException, SickTimeoutException );
00588   
00589     /** Synchronizes the driver state with the Sick LD (used for initialization) */
00590     void _syncDriverWithSick( ) throw( SickIOException, SickTimeoutException, SickErrorException );
00591 
00592     /** Set the function for a particular scan secto */
00593     void _setSickSectorFunction( const uint8_t sector_number, const uint8_t sector_function,
00594                                  const double sector_angle_stop, const bool write_to_flash = false )
00595        throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00596   
00597     /** Acquires the given Sector's function (i.e. current config) */
00598     void _getSickSectorFunction( const uint8_t sector_num, uint8_t &sector_function, double &sector_stop_angle )
00599       throw( SickErrorException, SickTimeoutException, SickIOException );
00600   
00601     /** Sets the Sick LD to IDLE mode */
00602     void _setSickSensorModeToIdle( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00603 
00604     /** Sets the Sick LD to ROTATE mode */
00605     void _setSickSensorModeToRotate( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00606 
00607     /** Sets the Sick LD to MEASURE mode */
00608     void _setSickSensorModeToMeasure( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00609   
00610     /** Sets the Sick LD's sensor mode to IDLE (laser off, motor off) */
00611     void _setSickSensorMode( const uint8_t new_sick_sensor_mode )
00612       throw( SickErrorException, SickTimeoutException, SickIOException );
00613   
00614     /** Requests n range measurement profiles from the Sick LD */
00615     void _getSickScanProfiles( const uint16_t profile_format, const uint16_t num_profiles = DEFAULT_SICK_NUM_SCAN_PROFILES )
00616       throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00617 
00618     /** Parses a sequence of bytes and populates the profile_data struct w/ the results */
00619     void _parseScanProfile( uint8_t * const src_buffer, sick_ld_scan_profile_t &profile_data ) const;
00620 
00621     /** Cancels the active data stream */
00622     void _cancelSickScanProfiles( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00623 
00624     /** Turns nearfield suppression on/off */
00625     void _setSickFilter( const uint8_t suppress_code )
00626       throw( SickErrorException, SickTimeoutException, SickIOException );
00627   
00628     /** Stores an image of the Sick LD's identity locally */
00629     void _getSickIdentity( ) throw( SickTimeoutException, SickIOException );
00630 
00631     /** Query the Sick for its sensor and motor status */
00632     void _getSickStatus( ) throw( SickTimeoutException, SickIOException );
00633 
00634     /** Sets the Sick LD's global configuration (in flash) */
00635     void _setSickGlobalConfig( const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step )
00636       throw( SickErrorException, SickTimeoutException, SickIOException );
00637 
00638     /** Query the Sick for its global configuration parameters */
00639     void _getSickGlobalConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00640 
00641     /** Query the Sick for its Ethernet configuration parameters */
00642     void _getSickEthernetConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00643 
00644     /** Acquires the configuration (function and stop angle) for each sector */
00645     void _getSickSectorConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00646   
00647     /** Query the Sick for ID information */
00648     void _getIdentificationString( const uint8_t id_request_code, std::string &id_return_string )
00649       throw( SickTimeoutException, SickIOException );
00650   
00651     /** Query the Sick for its sensor part number */
00652     void _getSensorPartNumber( ) throw( SickTimeoutException, SickIOException );
00653 
00654     /** Query the Sick for its assigned name */
00655     void _getSensorName( ) throw( SickTimeoutException, SickIOException );
00656 
00657     /** Query the Sick for its version number */
00658     void _getSensorVersion( ) throw( SickTimeoutException, SickIOException );
00659 
00660     /** Query the Sick for its serial number */
00661     void _getSensorSerialNumber( ) throw( SickTimeoutException, SickIOException );
00662 
00663     /** Query the Sick for its EDM unit's serial number */
00664     void _getSensorEDMSerialNumber( ) throw( SickTimeoutException, SickIOException );
00665 
00666     /** Query the Sick for the part number of its firmware */
00667     void _getFirmwarePartNumber( ) throw( SickTimeoutException, SickIOException );
00668 
00669     /** Query the Sick for the name of its firmware */
00670     void _getFirmwareName( ) throw( SickTimeoutException, SickIOException );
00671 
00672     /** Query the Sick for the version of the firmware */
00673     void _getFirmwareVersion( ) throw( SickTimeoutException, SickIOException );
00674 
00675     /** Query the part number of the application software */
00676     void _getApplicationSoftwarePartNumber( ) throw( SickTimeoutException, SickIOException );
00677 
00678     /** Query the Sick for the application name */
00679     void _getApplicationSoftwareName( ) throw( SickTimeoutException, SickIOException );
00680 
00681     /** Query the Sick for the application software version */
00682     void _getApplicationSoftwareVersion( ) throw( SickTimeoutException, SickIOException );
00683 
00684     /** Allows setting the global parameters and scan area definition (in flash) */
00685     void _setSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed, const double sick_step_angle,
00686                                            const double * const active_sector_start_angles,
00687                                            const double * const active_sector_stop_angles,
00688                                            const unsigned int num_active_sectors )
00689        throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
00690 
00691     /** Allows setting a temporary (until a device reset) sector configuration on the device */
00692     void _setSickTemporaryScanAreas( const double * const active_sector_start_angles,
00693                                      const double * const active_sector_stop_angles,
00694                                      const unsigned int num_active_sectors )
00695       throw( SickTimeoutException, SickIOException, SickConfigException );
00696 
00697     /** Sets the sick sector configuration */
00698     void _setSickSectorConfig( const unsigned int * const sector_functions, const double * const sector_stop_angles,
00699                                const unsigned int num_sectors, const bool write_to_flash = false )
00700       throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00701 
00702     /** Sets the signals for the device */
00703     void _setSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
00704       throw( SickIOException, SickTimeoutException, SickErrorException );
00705   
00706     /** Send a message, get the reply from the Sick LD and check it */
00707     void _sendMessageAndGetReply( const SickLDMessage &send_message, SickLDMessage &recv_message,
00708                                   const unsigned int timeout_value = DEFAULT_SICK_MESSAGE_TIMEOUT ) 
00709       throw( SickIOException, SickTimeoutException );
00710 
00711     /** Flushed the TCP receive buffer */
00712     void _flushTCPRecvBuffer( ) throw ( SickIOException, SickThreadException );
00713     
00714     /** Teardown the connection to the Sick LD */
00715     void _teardownConnection( ) throw( SickIOException );
00716 
00717     /** Generates a device-ready sector set given only an active sector spec. */
00718     void _generateSickSectorConfig( const double * const active_sector_start_angles,
00719                                     const double * const active_sector_stop_angles,
00720                                     const unsigned int num_active_sectors,
00721                                     const double sick_step_angle,
00722                                     unsigned int * const sector_functions, 
00723                                     double * const sector_stop_angles,
00724                                     unsigned int &num_sectors ) const;
00725   
00726     /** Converts odometry ticks to an equivalent angle */
00727     double _ticksToAngle( const uint16_t ticks ) const;
00728 
00729     /** Converts angle to an equivalent representation in odometer ticks */
00730     uint16_t _angleToTicks( const double angle ) const;
00731 
00732     /** Computes the mean pulse frequency for the given config */
00733     double _computeMeanPulseFrequency( const double active_scan_area, const double curr_motor_speed,
00734                                        const double curr_angular_resolution ) const;
00735   
00736     /** Computes the total pulse frequency for the given config */
00737     double _computeMaxPulseFrequency( const double total_scan_area, const double curr_motor_speed,
00738                                       const double curr_angular_resolution ) const;
00739 
00740     /** Indicates whether a given sensor ID is valid for the device */
00741     bool _validSickSensorID( const unsigned int sick_sensor_id ) const;
00742 
00743     /** Indicates whether a given motor speed is valid for the device */
00744     bool _validSickMotorSpeed( const unsigned int sick_motor_speed ) const;
00745 
00746     /** Indicates whether a given motor speed is valid for the device */
00747     bool _validSickScanResolution( const double sick_step_angle, const double * const active_sector_start_angles,
00748                                    const double * const active_sector_stop_angles, const unsigned int num_active_sectors ) const;
00749 
00750     /** Indicates whether the given configuration yields a valid max and mean pulse frequency */
00751     bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle ) const;
00752   
00753     /** Indicates whether the given configuration yields a valid max and mean pulse frequency */
00754     bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle,
00755                                const double * const active_sector_start_angles,
00756                                const double * const active_sector_stop_angles,
00757                                const unsigned int num_active_sectors ) const;
00758   
00759     /** Returns the scanning area for the device given the current sector configuration */
00760     double _computeScanArea( const double sick_step_angle, const double * const sector_start_angles,
00761                              const double * const sector_stop_angles, const unsigned int num_sectors ) const;
00762 
00763     /** Reorders given sector angle sets */
00764     void _sortScanAreas( double * const sector_start_angles, double * const sector_stop_angles,
00765                          const unsigned int num_sectors ) const;
00766 
00767     /** Checks the given sector arguments for overlapping regions yielding an invalid configuration */
00768     bool _validActiveSectors( const double * const sector_start_angles, const double * const sector_stop_angles,
00769                               const unsigned int num_active_sectors ) const;
00770     
00771     /** Indicates whether the supplied profile format is currently supported by the driver */
00772     bool _supportedScanProfileFormat( const uint16_t profile_format ) const; 
00773 
00774     /** Prints data corresponding to a single scan sector (data obtained using GET_PROFILE) */
00775     void _printSectorProfileData( const sick_ld_sector_data_t &sector_data ) const;
00776 
00777     /** Prints the data corresponding to the given scan profile (for debugging purposes) */
00778     void _printSickScanProfile( const sick_ld_scan_profile_t profile_data, const bool print_sector_data = true ) const;
00779 
00780     /** Returns the corresponding work service subcode required to transition the Sick LD to the given sensor mode. */
00781     uint8_t _sickSensorModeToWorkServiceSubcode( const uint8_t sick_sensor_mode ) const;
00782 
00783     /** Converts _sick_sensor_mode to a representative string */
00784     std::string _sickSensorModeToString( const uint8_t sick_sensor_mode ) const;
00785  
00786     /** Converts _sick_motor_mode to a representative string */
00787     std::string _sickMotorModeToString( const uint8_t sick_motor_mode ) const;
00788 
00789     /** Converts the specified trans measurement mode return value to a string */
00790     std::string _sickTransMeasureReturnToString( const uint8_t return_value ) const;
00791   
00792     /** Converts the specified reset level to a representative string */
00793     std::string _sickResetLevelToString( const uint16_t reset_level ) const;
00794   
00795     /** Converts Sick LD sector configuration word to a representative string */
00796     std::string _sickSectorFunctionToString( const uint16_t sick_sector_function ) const;
00797   
00798     /** Converts a given scan profile format to a string for friendlier output */
00799     std::string _sickProfileFormatToString( const uint16_t profile_format ) const;
00800 
00801     /** Prints the initialization footer */
00802     void _printInitFooter( ) const;
00803 
00804   };
00805 
00806 } //namespace SickToolbox
00807   
00808 #endif /* SICK_LD_HH */

Generated on Thu Mar 20 09:41:42 2008 for sicktoolbox-1.0 by  doxygen 1.5.1