00001
00017 #ifndef SICK_LMS_1XX_HH
00018 #define SICK_LMS_1XX_HH
00019
00020
00021 #define DEFAULT_SICK_LMS_1XX_IP_ADDRESS "192.168.0.1" ///< Default IP Address
00022 #define DEFAULT_SICK_LMS_1XX_TCP_PORT (2111) ///< Sick LMS 1xx TCP/IP Port
00023 #define DEFAULT_SICK_LMS_1XX_CONNECT_TIMEOUT (1000000) ///< Max time for establishing connection (usecs)
00024 #define DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT (5000000) ///< Max time for reply (usecs)
00025 #define DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT (60000000) ///< Max time it should take to change status
00026
00027 #define SICK_LMS_1XX_SCAN_AREA_MIN_ANGLE (-450000) ///< -45 degrees (1/10000) degree
00028 #define SICK_LMS_1XX_SCAN_AREA_MAX_ANGLE (2250000) ///< 225 degrees (1/10000) degree
00029
00030
00031 #include <string>
00032 #include <arpa/inet.h>
00033
00034 #include "SickLIDAR.hh"
00035 #include "SickLMS1xxBufferMonitor.hh"
00036 #include "SickLMS1xxMessage.hh"
00037 #include "SickException.hh"
00038
00043 namespace SickToolbox {
00044
00050 class SickLMS1xx : public SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage > {
00051
00052 public:
00053
00054 static const int SICK_LMS_1XX_MAX_NUM_MEASUREMENTS = 1082;
00055
00061 enum sick_lms_1xx_status_t {
00062
00063 SICK_LMS_1XX_STATUS_UNKNOWN = 0x00,
00064 SICK_LMS_1XX_STATUS_INITIALIZATION = 0x01,
00065 SICK_LMS_1XX_STATUS_CONFIGURATION = 0x02,
00066 SICK_LMS_1XX_STATUS_IDLE = 0x03,
00067 SICK_LMS_1XX_STATUS_ROTATED = 0x04,
00068 SICK_LMS_1XX_STATUS_IN_PREP = 0x05,
00069 SICK_LMS_1XX_STATUS_READY = 0x06,
00070 SICK_LMS_1XX_STATUS_READY_FOR_MEASUREMENT = 0x07
00071
00072 };
00073
00079 enum sick_lms_1xx_scan_format_t {
00080
00081 SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_NONE = 0x00,
00082 SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_8BIT = 0x01,
00083 SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_16BIT = 0x02,
00084 SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_NONE = 0x03,
00085 SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_8BIT = 0x04,
00086 SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT = 0x05,
00087 SICK_LMS_1XX_SCAN_FORMAT_UNKNOWN = 0xFF
00088
00089 };
00090
00096 enum sick_lms_1xx_scan_freq_t {
00097
00098 SICK_LMS_1XX_SCAN_FREQ_UNKNOWN = 0x00,
00099 SICK_LMS_1XX_SCAN_FREQ_25 = 0x09C4,
00100 SICK_LMS_1XX_SCAN_FREQ_50 = 0X1388
00101
00102 };
00103
00109 enum sick_lms_1xx_scan_res_t {
00110
00111 SICK_LMS_1XX_SCAN_RES_UNKNOWN = 0x00,
00112 SICK_LMS_1XX_SCAN_RES_25 = 0x09C4,
00113 SICK_LMS_1XX_SCAN_RES_50 = 0x1388
00114
00115 };
00116
00118 SickLMS1xx( const std::string sick_ip_address = DEFAULT_SICK_LMS_1XX_IP_ADDRESS,
00119 const uint16_t sick_tcp_port = DEFAULT_SICK_LMS_1XX_TCP_PORT );
00120
00122 void Initialize( const bool disp_banner = true ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
00123
00125 void SetSickScanFreqAndRes( const sick_lms_1xx_scan_freq_t scan_freq,
00126 const sick_lms_1xx_scan_res_t scan_res ) throw( SickTimeoutException, SickIOException, SickErrorException );
00127
00129 sick_lms_1xx_scan_freq_t GetSickScanFreq( ) const throw ( SickIOException );
00130
00132 sick_lms_1xx_scan_res_t GetSickScanRes( ) const throw ( SickIOException );
00133
00135 double GetSickStartAngle( ) const throw ( SickIOException );
00136
00138 double GetSickStopAngle( ) const throw ( SickIOException );
00139
00141 void SetSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException );
00142
00144 void GetSickMeasurements( unsigned int * const range_1_vals,
00145 unsigned int * const range_2_vals,
00146 unsigned int * const reflect_1_vals,
00147 unsigned int * const reflect_2_vals,
00148 unsigned int & num_measurements,
00149 unsigned int * const dev_status = NULL ) throw ( SickIOException, SickConfigException, SickTimeoutException );
00150
00152 void Uninitialize( const bool disp_banner = true ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
00153
00155 sick_lms_1xx_scan_freq_t IntToSickScanFreq( const int scan_freq ) const;
00156
00158 int SickScanFreqToInt( const sick_lms_1xx_scan_freq_t scan_freq ) const;
00159
00161 sick_lms_1xx_scan_res_t DoubleToSickScanRes( const double scan_res ) const;
00162
00164 double SickScanResToDouble( const sick_lms_1xx_scan_res_t scan_res ) const;
00165
00167 ~SickLMS1xx();
00168
00169 private:
00170
00180 typedef struct sick_lms_1xx_scan_config_tag {
00181 sick_lms_1xx_scan_freq_t sick_scan_freq;
00182 sick_lms_1xx_scan_res_t sick_scan_res;
00183 int32_t sick_start_angle;
00184 int32_t sick_stop_angle;
00185 } sick_lms_1xx_scan_config_t;
00186
00188 std::string _sick_ip_address;
00189
00191 uint16_t _sick_tcp_port;
00192
00194 struct sockaddr_in _sick_inet_address_info;
00195
00197 sick_lms_1xx_scan_config_t _sick_scan_config;
00198
00200 sick_lms_1xx_scan_format_t _sick_scan_format;
00201
00203 sick_lms_1xx_status_t _sick_device_status;
00204
00206 bool _sick_temp_safe;
00207
00209 bool _sick_streaming;
00210
00212 void _setupConnection( ) throw( SickIOException, SickTimeoutException );
00213
00215 void _reinitialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
00216
00218 void _teardownConnection( ) throw( SickIOException );
00219
00221 void _updateSickStatus( ) throw( SickTimeoutException, SickIOException );
00222
00224 void _getSickScanConfig( ) throw( SickTimeoutException, SickIOException );
00225
00227 void _setSickScanConfig( const sick_lms_1xx_scan_freq_t scan_freq,
00228 const sick_lms_1xx_scan_res_t scan_res,
00229 const int start_angle, const int stop_angle ) throw( SickTimeoutException, SickIOException, SickErrorException );
00230
00232 void _setAuthorizedClientAccessMode( ) throw( SickTimeoutException, SickErrorException, SickIOException );
00233
00235 void _writeToEEPROM( ) throw( SickTimeoutException, SickIOException );
00236
00238 void _sendMessage( const SickLMS1xxMessage &send_message ) const throw ( SickIOException );
00239
00241 void _sendMessageAndGetReply( const SickLMS1xxMessage &send_message,
00242 SickLMS1xxMessage &recv_message,
00243 const std::string reply_command_code,
00244 const std::string reply_command,
00245 const unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT,
00246 const unsigned int num_tries = 1 ) throw( SickIOException, SickTimeoutException );
00247
00249 void _recvMessage( SickLMS1xxMessage &sick_message ) const throw ( SickTimeoutException );
00250
00252 void _startMeasuring( ) throw ( SickTimeoutException, SickIOException );
00253
00255 void _stopMeasuring( ) throw ( SickTimeoutException, SickIOException );
00256
00258 void _requestDataStream( ) throw ( SickTimeoutException, SickConfigException, SickIOException );
00259
00261 void _startStreamingMeasurements( )throw( SickTimeoutException, SickIOException );
00262
00264 void _stopStreamingMeasurements( const bool disp_banner = true ) throw( SickTimeoutException, SickIOException );
00265
00267 void _checkForMeasuringStatus( unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT ) throw( SickTimeoutException, SickIOException );
00268
00270 void _setSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException );
00271
00273 void _restoreMeasuringMode( ) throw( SickTimeoutException, SickIOException );
00274
00276 bool _validScanArea( const int start_angle, const int stop_angle ) const;
00277
00279 sick_lms_1xx_status_t _intToSickStatus( const int status ) const;
00280
00282 void _printSickScanConfig( ) const;
00283
00285 void _printInitFooter( ) const;
00286
00288 std::string _sickScanDataFormatToString( const sick_lms_1xx_scan_format_t scan_format ) const;
00289
00291 double _convertSickAngleUnitsToDegs( const int sick_angle ) const { return ((double)sick_angle)/10000; }
00292
00294 unsigned int _convertSickFreqUnitsToHz( const unsigned int sick_freq ) const { return (unsigned int)(((double)sick_freq)/100); }
00295
00297 std::string _intToSickConfigErrorStr( const int error ) const;
00298
00300 bool _findSubString( const char * const str, const char * const substr, const unsigned int str_length, const unsigned int substr_length,
00301 unsigned int &substr_pos, unsigned int start_pos = 0 ) const;
00302
00304 char * _convertNextTokenToUInt( char * const str_buffer, unsigned int & num_val, const char * const delimeter = " " ) const;
00305
00306 };
00307
00312 typedef SickLMS1xx::sick_lms_1xx_status_t sick_lms_1xx_status_t;
00313
00318 typedef SickLMS1xx::sick_lms_1xx_scan_format_t sick_lms_1xx_scan_format_t;
00319
00324 typedef SickLMS1xx::sick_lms_1xx_scan_freq_t sick_lms_1xx_scan_freq_t;
00325
00330 typedef SickLMS1xx::sick_lms_1xx_scan_res_t sick_lms_1xx_scan_res_t;
00331
00332
00333 }
00334
00335 #endif