00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef SICK_LD_HH
00018 #define SICK_LD_HH
00019
00020
00021 #define DEFAULT_SICK_IP_ADDRESS "192.168.1.10"
00022 #define DEFAULT_SICK_TCP_PORT (49152)
00023 #define DEFAULT_SICK_MESSAGE_TIMEOUT (unsigned int)(5e6)
00024 #define DEFAULT_SICK_CONNECT_TIMEOUT (unsigned int)(1e6)
00025 #define DEFAULT_SICK_NUM_SCAN_PROFILES (0)
00026 #define DEFAULT_SICK_SIGNAL_SET (0)
00027
00028
00029
00030
00031
00032 #define SWAP_VALUES(x,y,t) (t=x,x=y,y=t);
00033
00034
00035 #include "SickConfig.hh"
00036
00037
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
00050
00051
00052 namespace SickToolbox {
00053
00054
00055
00056
00057
00058
00059 class SickLD : public SickLIDAR< SickLDBufferMonitor, SickLDMessage > {
00060
00061 public:
00062
00063
00064 static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 2881;
00065 static const uint16_t SICK_MAX_NUM_SECTORS = 8;
00066 static const uint16_t SICK_MAX_NUM_MEASURING_SECTORS = 4;
00067 static const uint16_t SICK_MAX_SCAN_AREA = 360;
00068 static const uint16_t SICK_MIN_MOTOR_SPEED = 5;
00069 static const uint16_t SICK_MAX_MOTOR_SPEED = 20;
00070 static const uint16_t SICK_MIN_VALID_SENSOR_ID = 1;
00071 static const uint16_t SICK_MAX_VALID_SENSOR_ID = 254;
00072 static const uint16_t SICK_MAX_MEAN_PULSE_FREQUENCY = 10800;
00073 static const uint16_t SICK_MAX_PULSE_FREQUENCY = 14400;
00074 static const uint16_t SICK_NUM_TICKS_PER_MOTOR_REV = 5760;
00075 static const double SICK_MAX_SCAN_ANGULAR_RESOLUTION = 0.125;
00076 static const double SICK_DEGREES_PER_MOTOR_STEP = 0.0625;
00077
00078
00079 static const uint8_t SICK_SENSOR_MODE_IDLE = 0x01;
00080 static const uint8_t SICK_SENSOR_MODE_ROTATE = 0x02;
00081 static const uint8_t SICK_SENSOR_MODE_MEASURE = 0x03;
00082 static const uint8_t SICK_SENSOR_MODE_ERROR = 0x04;
00083 static const uint8_t SICK_SENSOR_MODE_UNKNOWN = 0xFF;
00084
00085
00086 static const uint8_t SICK_MOTOR_MODE_OK = 0x00;
00087 static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_HIGH = 0x09;
00088 static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_LOW = 0x04;
00089 static const uint8_t SICK_MOTOR_MODE_ERROR = 0x0B;
00090 static const uint8_t SICK_MOTOR_MODE_UNKNOWN = 0xFF;
00091
00092
00093 static const uint8_t SICK_STAT_SERV_CODE = 0x01;
00094 static const uint8_t SICK_CONF_SERV_CODE = 0x02;
00095 static const uint8_t SICK_MEAS_SERV_CODE = 0x03;
00096 static const uint8_t SICK_WORK_SERV_CODE = 0x04;
00097 static const uint8_t SICK_ROUT_SERV_CODE = 0x06;
00098 static const uint8_t SICK_FILE_SERV_CODE = 0x07;
00099 static const uint8_t SICK_MONR_SERV_CODE = 0x08;
00100
00101
00102 static const uint8_t SICK_STAT_SERV_GET_ID = 0x01;
00103 static const uint8_t SICK_STAT_SERV_GET_STATUS = 0x02;
00104 static const uint8_t SICK_STAT_SERV_GET_SIGNAL = 0x04;
00105 static const uint8_t SICK_STAT_SERV_SET_SIGNAL = 0x05;
00106 static const uint8_t SICK_STAT_SERV_LD_REGISTER_APPLICATION = 0x06;
00107
00108
00109 static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM = 0x00;
00110 static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_NAME = 0x01;
00111 static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_VERSION = 0x02;
00112 static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM = 0x03;
00113 static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM = 0x04;
00114 static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM = 0x10;
00115 static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_NAME = 0x11;
00116 static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION = 0x12;
00117 static const uint8_t SICK_STAT_SERV_GET_ID_APP_PART_NUM = 0x20;
00118 static const uint8_t SICK_STAT_SERV_GET_ID_APP_NAME = 0x21;
00119 static const uint8_t SICK_STAT_SERV_GET_ID_APP_VERSION = 0x22;
00120
00121
00122 static const uint8_t SICK_CONF_SERV_SET_CONFIGURATION = 0x01;
00123 static const uint8_t SICK_CONF_SERV_GET_CONFIGURATION = 0x02;
00124 static const uint8_t SICK_CONF_SERV_SET_TIME_ABSOLUTE = 0x03;
00125 static const uint8_t SICK_CONF_SERV_SET_TIME_RELATIVE = 0x04;
00126 static const uint8_t SICK_CONF_SERV_GET_SYNC_CLOCK = 0x05;
00127 static const uint8_t SICK_CONF_SERV_SET_FILTER = 0x09;
00128 static const uint8_t SICK_CONF_SERV_SET_FUNCTION = 0x0A;
00129 static const uint8_t SICK_CONF_SERV_GET_FUNCTION = 0x0B;
00130
00131
00132 static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD = 0x01;
00133
00134
00135 static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF = 0x00;
00136 static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON = 0x01;
00137
00138
00139 static const uint8_t SICK_MEAS_SERV_GET_PROFILE = 0x01;
00140 static const uint8_t SICK_MEAS_SERV_CANCEL_PROFILE = 0x02;
00141
00142
00143 static const uint8_t SICK_WORK_SERV_RESET = 0x01;
00144 static const uint8_t SICK_WORK_SERV_TRANS_IDLE = 0x02;
00145 static const uint8_t SICK_WORK_SERV_TRANS_ROTATE = 0x03;
00146 static const uint8_t SICK_WORK_SERV_TRANS_MEASURE = 0x04;
00147
00148
00149 static const uint8_t SICK_WORK_SERV_RESET_INIT_CPU = 0x00;
00150 static const uint8_t SICK_WORK_SERV_RESET_KEEP_CPU = 0x01;
00151 static const uint8_t SICK_WORK_SERV_RESET_HALT_APP = 0x02;
00152
00153
00154 static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_OK = 0x00;
00155 static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE = 0x01;
00156 static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE = 0x02;
00157 static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER = 0x03;
00158 static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT = 0x04;
00159
00160
00161 static const uint8_t SICK_ROUT_SERV_COM_ATTACH = 0x01;
00162 static const uint8_t SICK_ROUT_SERV_COM_DETACH = 0x02;
00163 static const uint8_t SICK_ROUT_SERV_COM_INITIALIZE = 0x03;
00164 static const uint8_t SICK_ROUT_SERV_COM_OUTPUT = 0x04;
00165 static const uint8_t SICK_ROUT_SERV_COM_DATA = 0x05;
00166
00167
00168 static const uint8_t SICK_FILE_SERV_DIR = 0x01;
00169 static const uint8_t SICK_FILE_SERV_SAVE = 0x02;
00170 static const uint8_t SICK_FILE_SERV_LOAD = 0x03;
00171 static const uint8_t SICK_FILE_SERV_DELETE = 0x04;
00172
00173
00174 static const uint8_t SICK_MONR_SERV_MONITOR_RUN = 0x01;
00175 static const uint8_t SICK_MONR_SERV_MONITOR_PROFILE_LOG = 0x02;
00176
00177
00178 static const uint8_t SICK_CONF_KEY_RS232_RS422 = 0x01;
00179 static const uint8_t SICK_CONF_KEY_CAN = 0x02;
00180 static const uint8_t SICK_CONF_KEY_ETHERNET = 0x05;
00181 static const uint8_t SICK_CONF_KEY_GLOBAL = 0x10;
00182
00183
00184 static const uint8_t SICK_CONF_SECTOR_NOT_INITIALIZED = 0x00;
00185 static const uint8_t SICK_CONF_SECTOR_NO_MEASUREMENT = 0x01;
00186 static const uint8_t SICK_CONF_SECTOR_RESERVED = 0x02;
00187 static const uint8_t SICK_CONF_SECTOR_NORMAL_MEASUREMENT = 0x03;
00188 static const uint8_t SICK_CONF_SECTOR_REFERENCE_MEASUREMENT = 0x04;
00189
00190
00191 static const uint16_t SICK_SCAN_PROFILE_RANGE = 0x39FF;
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 static const uint16_t SICK_SCAN_PROFILE_RANGE_AND_ECHO = 0x3DFF;
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245 static const uint8_t SICK_SIGNAL_LED_YELLOW_A = 0x01;
00246 static const uint8_t SICK_SIGNAL_LED_YELLOW_B = 0x02;
00247 static const uint8_t SICK_SIGNAL_LED_GREEN = 0x04;
00248 static const uint8_t SICK_SIGNAL_LED_RED = 0x08;
00249 static const uint8_t SICK_SIGNAL_SWITCH_0 = 0x10;
00250 static const uint8_t SICK_SIGNAL_SWITCH_1 = 0x20;
00251 static const uint8_t SICK_SIGNAL_SWITCH_2 = 0x40;
00252 static const uint8_t SICK_SIGNAL_SWITCH_3 = 0x80;
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 typedef struct sick_ld_config_global_tag {
00264 uint16_t sick_sensor_id;
00265 uint16_t sick_motor_speed;
00266 double sick_angle_step;
00267 } sick_ld_config_global_t;
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280 typedef struct sick_ld_config_ethernet_tag {
00281 uint16_t sick_ip_address[4];
00282 uint16_t sick_subnet_mask[4];
00283 uint16_t sick_gateway_ip_address[4];
00284 uint16_t sick_node_id;
00285 uint16_t sick_transparent_tcp_port;
00286 } sick_ld_config_ethernet_t;
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297 typedef struct sick_ld_config_sector_tag {
00298 uint8_t sick_num_active_sectors;
00299 uint8_t sick_num_initialized_sectors;
00300 uint8_t sick_active_sector_ids[SICK_MAX_NUM_SECTORS];
00301 uint8_t sick_sector_functions[SICK_MAX_NUM_SECTORS];
00302 double sick_sector_start_angles[SICK_MAX_NUM_SECTORS];
00303 double sick_sector_stop_angles[SICK_MAX_NUM_SECTORS];
00304 } sick_ld_config_sector_t;
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315 typedef struct sick_ld_identity_tag {
00316 std::string sick_part_number;
00317 std::string sick_name;
00318 std::string sick_version;
00319 std::string sick_serial_number;
00320 std::string sick_edm_serial_number;
00321 std::string sick_firmware_part_number;
00322 std::string sick_firmware_name;
00323 std::string sick_firmware_version;
00324 std::string sick_application_software_part_number;
00325 std::string sick_application_software_name;
00326 std::string sick_application_software_version;
00327 } sick_ld_identity_t;
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338 typedef struct sick_ld_sector_data_tag {
00339 unsigned int sector_num;
00340 unsigned int num_data_points;
00341 unsigned int timestamp_start;
00342 unsigned int timestamp_stop;
00343 unsigned int echo_values[SICK_MAX_NUM_MEASUREMENTS];
00344 double angle_step;
00345 double angle_start;
00346 double angle_stop;
00347 double range_values[SICK_MAX_NUM_MEASUREMENTS];
00348 double scan_angles[SICK_MAX_NUM_MEASUREMENTS];
00349 } sick_ld_sector_data_t;
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361 typedef struct sick_ld_scan_profile_tag {
00362 unsigned int profile_number;
00363 unsigned int profile_counter;
00364 unsigned int layer_num;
00365 unsigned int sensor_status;
00366 unsigned int motor_status;
00367 unsigned int num_sectors;
00368 sick_ld_sector_data_t sector_data[SICK_MAX_NUM_SECTORS];
00369 } sick_ld_scan_profile_t;
00370
00371
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
00376 void Initialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
00377
00378
00379 void GetSickStatus( unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode )
00380 throw( SickIOException, SickTimeoutException );
00381
00382
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
00388 void SetSickTimeAbsolute( const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time )
00389 throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00390
00391
00392 void SetSickTimeRelative( const int16_t time_delta, uint16_t &new_sick_clock_time )
00393 throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00394
00395
00396 void GetSickTime( uint16_t &sick_time )
00397 throw( SickIOException, SickTimeoutException, SickErrorException );
00398
00399
00400 void SetSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
00401 throw( SickIOException, SickTimeoutException, SickErrorException );
00402
00403
00404 void GetSickSignals( uint8_t &sick_signal_flags ) throw( SickIOException, SickTimeoutException );
00405
00406
00407 void EnableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00408
00409
00410 void DisableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00411
00412
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
00426 void SetSickSensorID( const unsigned int sick_sensor_id )
00427 throw( SickErrorException, SickTimeoutException, SickIOException );
00428
00429
00430 void SetSickMotorSpeed( const unsigned int sick_motor_speed )
00431 throw( SickErrorException, SickTimeoutException, SickIOException );
00432
00433
00434 void SetSickScanResolution( const double sick_step_angle )
00435 throw( SickTimeoutException, SickIOException, SickConfigException );
00436
00437
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
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
00452 void ResetSick( const unsigned int reset_level = SICK_WORK_SERV_RESET_INIT_CPU )
00453 throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
00454
00455
00456 unsigned int GetSickNumActiveSectors( ) const;
00457
00458
00459 unsigned int GetSickSensorID( ) const;
00460
00461
00462 unsigned int GetSickMotorSpeed( ) const;
00463
00464
00465 double GetSickScanResolution( ) const;
00466
00467
00468 std::string GetSickIPAddress( ) const;
00469
00470
00471 std::string GetSickSubnetMask( ) const;
00472
00473
00474 std::string GetSickGatewayIPAddress( ) const;
00475
00476
00477 std::string GetSickPartNumber( ) const;
00478
00479
00480 std::string GetSickName( ) const;
00481
00482
00483 std::string GetSickVersion( ) const;
00484
00485
00486 std::string GetSickSerialNumber( ) const;
00487
00488
00489 std::string GetSickEDMSerialNumber( ) const;
00490
00491
00492 std::string GetSickFirmwarePartNumber( ) const;
00493
00494
00495 std::string GetSickFirmwareName( ) const;
00496
00497
00498 std::string GetSickFirmwareVersion( ) const;
00499
00500
00501 std::string GetSickAppSoftwarePartNumber( ) const;
00502
00503
00504 std::string GetSickAppSoftwareName( ) const;
00505
00506
00507 std::string GetSickAppSoftwareVersionNumber( ) const;
00508
00509
00510 std::string GetSickStatusAsString() const;
00511
00512
00513 std::string GetSickIdentityAsString() const;
00514
00515
00516 std::string GetSickGlobalConfigAsString() const;
00517
00518
00519 std::string GetSickEthernetConfigAsString() const;
00520
00521
00522 std::string GetSickSectorConfigAsString() const;
00523
00524
00525 double GetSickScanArea( ) const;
00526
00527
00528 void PrintSickStatus( ) const;
00529
00530
00531 void PrintSickIdentity( ) const;
00532
00533
00534 void PrintSickGlobalConfig( ) const;
00535
00536
00537 void PrintSickEthernetConfig( ) const;
00538
00539
00540 void PrintSickSectorConfig( ) const;
00541
00542
00543 void Uninitialize( ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
00544
00545
00546 ~SickLD();
00547
00548 private:
00549
00550
00551 std::string _sick_ip_address;
00552
00553
00554 uint16_t _sick_tcp_port;
00555
00556
00557 unsigned int _socket;
00558
00559
00560 struct sockaddr_in _sick_inet_address_info;
00561
00562
00563 uint8_t _sick_sensor_mode;
00564
00565
00566 uint8_t _sick_motor_mode;
00567
00568
00569 bool _sick_streaming_range_data;
00570
00571
00572 bool _sick_streaming_range_and_echo_data;
00573
00574
00575 sick_ld_identity_t _sick_identity;
00576
00577
00578 sick_ld_config_global_t _sick_global_config;
00579
00580
00581 sick_ld_config_ethernet_t _sick_ethernet_config;
00582
00583
00584 sick_ld_config_sector_t _sick_sector_config;
00585
00586
00587 void _setupConnection( ) throw( SickIOException, SickTimeoutException );
00588
00589
00590 void _syncDriverWithSick( ) throw( SickIOException, SickTimeoutException, SickErrorException );
00591
00592
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
00598 void _getSickSectorFunction( const uint8_t sector_num, uint8_t §or_function, double §or_stop_angle )
00599 throw( SickErrorException, SickTimeoutException, SickIOException );
00600
00601
00602 void _setSickSensorModeToIdle( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00603
00604
00605 void _setSickSensorModeToRotate( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00606
00607
00608 void _setSickSensorModeToMeasure( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00609
00610
00611 void _setSickSensorMode( const uint8_t new_sick_sensor_mode )
00612 throw( SickErrorException, SickTimeoutException, SickIOException );
00613
00614
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
00619 void _parseScanProfile( uint8_t * const src_buffer, sick_ld_scan_profile_t &profile_data ) const;
00620
00621
00622 void _cancelSickScanProfiles( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00623
00624
00625 void _setSickFilter( const uint8_t suppress_code )
00626 throw( SickErrorException, SickTimeoutException, SickIOException );
00627
00628
00629 void _getSickIdentity( ) throw( SickTimeoutException, SickIOException );
00630
00631
00632 void _getSickStatus( ) throw( SickTimeoutException, SickIOException );
00633
00634
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
00639 void _getSickGlobalConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00640
00641
00642 void _getSickEthernetConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00643
00644
00645 void _getSickSectorConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
00646
00647
00648 void _getIdentificationString( const uint8_t id_request_code, std::string &id_return_string )
00649 throw( SickTimeoutException, SickIOException );
00650
00651
00652 void _getSensorPartNumber( ) throw( SickTimeoutException, SickIOException );
00653
00654
00655 void _getSensorName( ) throw( SickTimeoutException, SickIOException );
00656
00657
00658 void _getSensorVersion( ) throw( SickTimeoutException, SickIOException );
00659
00660
00661 void _getSensorSerialNumber( ) throw( SickTimeoutException, SickIOException );
00662
00663
00664 void _getSensorEDMSerialNumber( ) throw( SickTimeoutException, SickIOException );
00665
00666
00667 void _getFirmwarePartNumber( ) throw( SickTimeoutException, SickIOException );
00668
00669
00670 void _getFirmwareName( ) throw( SickTimeoutException, SickIOException );
00671
00672
00673 void _getFirmwareVersion( ) throw( SickTimeoutException, SickIOException );
00674
00675
00676 void _getApplicationSoftwarePartNumber( ) throw( SickTimeoutException, SickIOException );
00677
00678
00679 void _getApplicationSoftwareName( ) throw( SickTimeoutException, SickIOException );
00680
00681
00682 void _getApplicationSoftwareVersion( ) throw( SickTimeoutException, SickIOException );
00683
00684
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
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
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
00703 void _setSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
00704 throw( SickIOException, SickTimeoutException, SickErrorException );
00705
00706
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
00712 void _flushTCPRecvBuffer( ) throw ( SickIOException, SickThreadException );
00713
00714
00715 void _teardownConnection( ) throw( SickIOException );
00716
00717
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
00727 double _ticksToAngle( const uint16_t ticks ) const;
00728
00729
00730 uint16_t _angleToTicks( const double angle ) const;
00731
00732
00733 double _computeMeanPulseFrequency( const double active_scan_area, const double curr_motor_speed,
00734 const double curr_angular_resolution ) const;
00735
00736
00737 double _computeMaxPulseFrequency( const double total_scan_area, const double curr_motor_speed,
00738 const double curr_angular_resolution ) const;
00739
00740
00741 bool _validSickSensorID( const unsigned int sick_sensor_id ) const;
00742
00743
00744 bool _validSickMotorSpeed( const unsigned int sick_motor_speed ) const;
00745
00746
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
00751 bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle ) const;
00752
00753
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
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
00764 void _sortScanAreas( double * const sector_start_angles, double * const sector_stop_angles,
00765 const unsigned int num_sectors ) const;
00766
00767
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
00772 bool _supportedScanProfileFormat( const uint16_t profile_format ) const;
00773
00774
00775 void _printSectorProfileData( const sick_ld_sector_data_t §or_data ) const;
00776
00777
00778 void _printSickScanProfile( const sick_ld_scan_profile_t profile_data, const bool print_sector_data = true ) const;
00779
00780
00781 uint8_t _sickSensorModeToWorkServiceSubcode( const uint8_t sick_sensor_mode ) const;
00782
00783
00784 std::string _sickSensorModeToString( const uint8_t sick_sensor_mode ) const;
00785
00786
00787 std::string _sickMotorModeToString( const uint8_t sick_motor_mode ) const;
00788
00789
00790 std::string _sickTransMeasureReturnToString( const uint8_t return_value ) const;
00791
00792
00793 std::string _sickResetLevelToString( const uint16_t reset_level ) const;
00794
00795
00796 std::string _sickSectorFunctionToString( const uint16_t sick_sector_function ) const;
00797
00798
00799 std::string _sickProfileFormatToString( const uint16_t profile_format ) const;
00800
00801
00802 void _printInitFooter( ) const;
00803
00804 };
00805
00806 }
00807
00808 #endif