SickLD.hh
Go to the documentation of this file.
1 
17 #ifndef SICK_LD_HH
18 #define SICK_LD_HH
19 
20 /* Macros */
21 #define DEFAULT_SICK_IP_ADDRESS "192.168.1.10"
22 #define DEFAULT_SICK_TCP_PORT (49152)
23 #define DEFAULT_SICK_MESSAGE_TIMEOUT (unsigned int)(5e6)
24 #define DEFAULT_SICK_CONNECT_TIMEOUT (unsigned int)(1e6)
25 #define DEFAULT_SICK_NUM_SCAN_PROFILES (0)
26 #define DEFAULT_SICK_SIGNAL_SET (0)
27 
28 
32 #define SWAP_VALUES(x,y,t) (t=x,x=y,y=t);
33 
34 /* Definition dependencies */
35 #include <string>
36 #include <vector>
37 #include <pthread.h>
38 #include <arpa/inet.h>
39 
40 #include "SickLIDAR.hh"
41 #include "SickLDBufferMonitor.hh"
42 #include "SickLDMessage.hh"
43 #include "SickException.hh"
44 
49 namespace SickToolbox {
50 
56  class SickLD : public SickLIDAR< SickLDBufferMonitor, SickLDMessage > {
57 
58  public:
59 
60  /* Some constants for the developer/end-user */
61  static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 2881;
62  static const uint16_t SICK_MAX_NUM_SECTORS = 8;
63  static const uint16_t SICK_MAX_NUM_MEASURING_SECTORS = 4;
64  static const uint16_t SICK_MAX_SCAN_AREA = 360;
65  static const uint16_t SICK_MIN_MOTOR_SPEED = 5;
66  static const uint16_t SICK_MAX_MOTOR_SPEED = 20;
67  static const uint16_t SICK_MIN_VALID_SENSOR_ID = 1;
68  static const uint16_t SICK_MAX_VALID_SENSOR_ID = 254;
69  static const uint16_t SICK_MAX_MEAN_PULSE_FREQUENCY = 10800;
70  static const uint16_t SICK_MAX_PULSE_FREQUENCY = 14400;
71  static const uint16_t SICK_NUM_TICKS_PER_MOTOR_REV = 5760;
72  static constexpr double SICK_MAX_SCAN_ANGULAR_RESOLUTION = 0.125;
73  static constexpr double SICK_DEGREES_PER_MOTOR_STEP = 0.0625;
74 
75  /* Sick LD sensor modes of operation */
76  static const uint8_t SICK_SENSOR_MODE_IDLE = 0x01;
77  static const uint8_t SICK_SENSOR_MODE_ROTATE = 0x02;
78  static const uint8_t SICK_SENSOR_MODE_MEASURE = 0x03;
79  static const uint8_t SICK_SENSOR_MODE_ERROR = 0x04;
80  static const uint8_t SICK_SENSOR_MODE_UNKNOWN = 0xFF;
81 
82  /* Sick LD motor modes */
83  static const uint8_t SICK_MOTOR_MODE_OK = 0x00;
84  static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_HIGH = 0x09;
85  static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_LOW = 0x04;
86  static const uint8_t SICK_MOTOR_MODE_ERROR = 0x0B;
87  static const uint8_t SICK_MOTOR_MODE_UNKNOWN = 0xFF;
88 
89  /* Sick LD service codes */
90  static const uint8_t SICK_STAT_SERV_CODE = 0x01;
91  static const uint8_t SICK_CONF_SERV_CODE = 0x02;
92  static const uint8_t SICK_MEAS_SERV_CODE = 0x03;
93  static const uint8_t SICK_WORK_SERV_CODE = 0x04;
94  static const uint8_t SICK_ROUT_SERV_CODE = 0x06;
95  static const uint8_t SICK_FILE_SERV_CODE = 0x07;
96  static const uint8_t SICK_MONR_SERV_CODE = 0x08;
97 
98  /* Sick LD status services (service code 0x01) */
99  static const uint8_t SICK_STAT_SERV_GET_ID = 0x01;
100  static const uint8_t SICK_STAT_SERV_GET_STATUS = 0x02;
101  static const uint8_t SICK_STAT_SERV_GET_SIGNAL = 0x04;
102  static const uint8_t SICK_STAT_SERV_SET_SIGNAL = 0x05;
103  static const uint8_t SICK_STAT_SERV_LD_REGISTER_APPLICATION = 0x06;
104 
105  /* Sick LD status service GET_IDENTIFICATION request codes */
106  static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM = 0x00;
107  static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_NAME = 0x01;
108  static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_VERSION = 0x02;
109  static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM = 0x03;
110  static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM = 0x04;
111  static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM = 0x10;
112  static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_NAME = 0x11;
113  static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION = 0x12;
114  static const uint8_t SICK_STAT_SERV_GET_ID_APP_PART_NUM = 0x20;
115  static const uint8_t SICK_STAT_SERV_GET_ID_APP_NAME = 0x21;
116  static const uint8_t SICK_STAT_SERV_GET_ID_APP_VERSION = 0x22;
117 
118  /* Sick LD configuration services (service code 0x02) */
119  static const uint8_t SICK_CONF_SERV_SET_CONFIGURATION = 0x01;
120  static const uint8_t SICK_CONF_SERV_GET_CONFIGURATION = 0x02;
121  static const uint8_t SICK_CONF_SERV_SET_TIME_ABSOLUTE = 0x03;
122  static const uint8_t SICK_CONF_SERV_SET_TIME_RELATIVE = 0x04;
123  static const uint8_t SICK_CONF_SERV_GET_SYNC_CLOCK = 0x05;
124  static const uint8_t SICK_CONF_SERV_SET_FILTER = 0x09;
125  static const uint8_t SICK_CONF_SERV_SET_FUNCTION = 0x0A;
126  static const uint8_t SICK_CONF_SERV_GET_FUNCTION = 0x0B;
127 
128  /* Sick LD configuration filter codes */
129  static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD = 0x01;
130 
131  /* Sick LD nearfield suppression configuration codes */
132  static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF = 0x00;
133  static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON = 0x01;
134 
135  /* Sick LD measurement services (service code 0x03) */
136  static const uint8_t SICK_MEAS_SERV_GET_PROFILE = 0x01;
137  static const uint8_t SICK_MEAS_SERV_CANCEL_PROFILE = 0x02;
138 
139  /* Sick LD working services (service code 0x04) */
140  static const uint8_t SICK_WORK_SERV_RESET = 0x01;
141  static const uint8_t SICK_WORK_SERV_TRANS_IDLE = 0x02;
142  static const uint8_t SICK_WORK_SERV_TRANS_ROTATE = 0x03;
143  static const uint8_t SICK_WORK_SERV_TRANS_MEASURE = 0x04;
144 
145  /* Sick LD working service DO_RESET request codes */
146  static const uint8_t SICK_WORK_SERV_RESET_INIT_CPU = 0x00;
147  static const uint8_t SICK_WORK_SERV_RESET_KEEP_CPU = 0x01;
148  static const uint8_t SICK_WORK_SERV_RESET_HALT_APP = 0x02;
149 
150  /* Sick LD working service TRANS_MEASURE return codes */
151  static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_OK = 0x00;
156 
157  /* Sick LD interface routing services (service code 0x06) */
158  static const uint8_t SICK_ROUT_SERV_COM_ATTACH = 0x01;
159  static const uint8_t SICK_ROUT_SERV_COM_DETACH = 0x02;
160  static const uint8_t SICK_ROUT_SERV_COM_INITIALIZE = 0x03;
161  static const uint8_t SICK_ROUT_SERV_COM_OUTPUT = 0x04;
162  static const uint8_t SICK_ROUT_SERV_COM_DATA = 0x05;
163 
164  /* Sick LD file services (service code 0x07) */
165  static const uint8_t SICK_FILE_SERV_DIR = 0x01;
166  static const uint8_t SICK_FILE_SERV_SAVE = 0x02;
167  static const uint8_t SICK_FILE_SERV_LOAD = 0x03;
168  static const uint8_t SICK_FILE_SERV_DELETE = 0x04;
169 
170  /* Sick LD monitor services (service code 0x08) */
171  static const uint8_t SICK_MONR_SERV_MONITOR_RUN = 0x01;
172  static const uint8_t SICK_MONR_SERV_MONITOR_PROFILE_LOG = 0x02;
173 
174  /* Sick LD configuration keys */
175  static const uint8_t SICK_CONF_KEY_RS232_RS422 = 0x01;
176  static const uint8_t SICK_CONF_KEY_CAN = 0x02;
177  static const uint8_t SICK_CONF_KEY_ETHERNET = 0x05;
178  static const uint8_t SICK_CONF_KEY_GLOBAL = 0x10;
179 
180  /* Sick LD sector configuration codes */
181  static const uint8_t SICK_CONF_SECTOR_NOT_INITIALIZED = 0x00;
182  static const uint8_t SICK_CONF_SECTOR_NO_MEASUREMENT = 0x01;
183  static const uint8_t SICK_CONF_SECTOR_RESERVED = 0x02;
184  static const uint8_t SICK_CONF_SECTOR_NORMAL_MEASUREMENT = 0x03;
185  static const uint8_t SICK_CONF_SECTOR_REFERENCE_MEASUREMENT = 0x04;
186 
187  /* Sick LD profile formats */
188  static const uint16_t SICK_SCAN_PROFILE_RANGE = 0x39FF;
189  /*
190  * SICK_SCAN_PROFILE_RANGE format (0x39FF) interpretation:
191  * (See page 32 of telegram listing for fieldname definitions)
192  *
193  * Field Name | Send
194  * --------------------
195  * PROFILESENT | YES
196  * PROFILECOUNT | YES
197  * LAYERNUM | YES
198  * SECTORNUM | YES
199  * DIRSTEP | YES
200  * POINTNUM | YES
201  * TSTART | YES
202  * STARTDIR | YES
203  * DISTANCE-n | YES
204  * DIRECTION-n | NO
205  * ECHO-n | NO
206  * TEND | YES
207  * ENDDIR | YES
208  * SENSTAT | YES
209  */
210 
211  /* Sick LD profile formats */
212  static const uint16_t SICK_SCAN_PROFILE_RANGE_AND_ECHO = 0x3DFF;
213  /*
214  * SICK_SCAN_PROFILE_RANGE format (0x3DFF) interpretation:
215  * (See page 32 of telegram listing for fieldname definitions)
216  *
217  * Field Name | Send
218  * --------------------
219  * PROFILESENT | YES
220  * PROFILECOUNT | YES
221  * LAYERNUM | YES
222  * SECTORNUM | YES
223  * DIRSTEP | YES
224  * POINTNUM | YES
225  * TSTART | YES
226  * STARTDIR | YES
227  * DISTANCE-n | YES
228  * DIRECTION-n | NO
229  * ECHO-n | YES
230  * TEND | YES
231  * ENDDIR | YES
232  * SENSTAT | YES
233  */
234 
235  /* Masks for working with the Sick LD signals
236  *
237  * NOTE: Although the Sick LD manual defines the flag
238  * values for red and green LEDs the operation
239  * of these LEDs are reserved. So they can't
240  * be set by the device driver.
241  */
242  static const uint8_t SICK_SIGNAL_LED_YELLOW_A = 0x01;
243  static const uint8_t SICK_SIGNAL_LED_YELLOW_B = 0x02;
244  static const uint8_t SICK_SIGNAL_LED_GREEN = 0x04;
245  static const uint8_t SICK_SIGNAL_LED_RED = 0x08;
246  static const uint8_t SICK_SIGNAL_SWITCH_0 = 0x10;
247  static const uint8_t SICK_SIGNAL_SWITCH_1 = 0x20;
248  static const uint8_t SICK_SIGNAL_SWITCH_2 = 0x40;
249  static const uint8_t SICK_SIGNAL_SWITCH_3 = 0x80;
250 
260  typedef struct sick_ld_config_global_tag {
261  uint16_t sick_sensor_id;
262  uint16_t sick_motor_speed;
265 
278  uint16_t sick_ip_address[4];
279  uint16_t sick_subnet_mask[4];
280  uint16_t sick_gateway_ip_address[4];
281  uint16_t sick_node_id;
284 
294  typedef struct sick_ld_config_sector_tag {
297  uint8_t sick_active_sector_ids[SICK_MAX_NUM_SECTORS];
298  uint8_t sick_sector_functions[SICK_MAX_NUM_SECTORS];
299  double sick_sector_start_angles[SICK_MAX_NUM_SECTORS];
300  double sick_sector_stop_angles[SICK_MAX_NUM_SECTORS];
302 
312  typedef struct sick_ld_identity_tag {
313  std::string sick_part_number;
314  std::string sick_name;
315  std::string sick_version;
316  std::string sick_serial_number;
319  std::string sick_firmware_name;
320  std::string sick_firmware_version;
325 
335  typedef struct sick_ld_sector_data_tag {
336  unsigned int sector_num;
337  unsigned int num_data_points;
338  unsigned int timestamp_start;
339  unsigned int timestamp_stop;
340  unsigned int echo_values[SICK_MAX_NUM_MEASUREMENTS];
341  double angle_step;
342  double angle_start;
343  double angle_stop;
344  double range_values[SICK_MAX_NUM_MEASUREMENTS];
345  double scan_angles[SICK_MAX_NUM_MEASUREMENTS];
347 
358  typedef struct sick_ld_scan_profile_tag {
359  unsigned int profile_number;
360  unsigned int profile_counter;
361  unsigned int layer_num;
362  unsigned int sensor_status;
363  unsigned int motor_status;
364  unsigned int num_sectors;
367 
369  SickLD( const std::string sick_ip_address = DEFAULT_SICK_IP_ADDRESS,
370  const uint16_t sick_tcp_port = DEFAULT_SICK_TCP_PORT );
371 
374 
376  void GetSickStatus( unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode )
377  throw( SickIOException, SickTimeoutException );
378 
380  void SetSickTempScanAreas( const double * active_sector_start_angles, const double * const active_sector_stop_angles,
381  const unsigned int num_active_sectors )
382  throw( SickTimeoutException, SickIOException, SickConfigException );
383 
385  void SetSickTimeAbsolute( const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time )
386  throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
387 
389  void SetSickTimeRelative( const int16_t time_delta, uint16_t &new_sick_clock_time )
390  throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
391 
393  void GetSickTime( uint16_t &sick_time )
394  throw( SickIOException, SickTimeoutException, SickErrorException );
395 
397  void SetSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
398  throw( SickIOException, SickTimeoutException, SickErrorException );
399 
401  void GetSickSignals( uint8_t &sick_signal_flags ) throw( SickIOException, SickTimeoutException );
402 
404  void EnableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
405 
407  void DisableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
408 
410  void GetSickMeasurements( double * const range_measurements,
411  unsigned int * const echo_measurements = NULL,
412  unsigned int * const num_measurements = NULL,
413  unsigned int * const sector_ids = NULL,
414  unsigned int * const sector_data_offsets = NULL,
415  double * const sector_step_angles = NULL,
416  double * const sector_start_angles = NULL,
417  double * const sector_stop_angles = NULL,
418  unsigned int * const sector_start_timestamps = NULL,
419  unsigned int * const sector_stop_timestamps = NULL )
420  throw( SickErrorException, SickIOException, SickTimeoutException, SickConfigException );
421 
423  void SetSickSensorID( const unsigned int sick_sensor_id )
424  throw( SickErrorException, SickTimeoutException, SickIOException );
425 
427  void SetSickMotorSpeed( const unsigned int sick_motor_speed )
428  throw( SickErrorException, SickTimeoutException, SickIOException );
429 
431  void SetSickScanResolution( const double sick_step_angle )
432  throw( SickTimeoutException, SickIOException, SickConfigException );
433 
435  void SetSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed,
436  const double sick_step_angle,
437  const double * const active_sector_start_angles,
438  const double * const active_sector_stop_angles,
439  const unsigned int num_active_sectors )
440  throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
441 
443  void SetSickScanAreas( const double * const active_sector_start_angles,
444  const double * const active_sector_stop_angles,
445  const unsigned int num_active_sectors )
446  throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
447 
449  void ResetSick( const unsigned int reset_level = SICK_WORK_SERV_RESET_INIT_CPU )
450  throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
451 
453  unsigned int GetSickNumActiveSectors( ) const;
454 
456  unsigned int GetSickSensorID( ) const;
457 
459  unsigned int GetSickMotorSpeed( ) const;
460 
462  double GetSickScanResolution( ) const;
463 
465  std::string GetSickIPAddress( ) const;
466 
468  std::string GetSickSubnetMask( ) const;
469 
471  std::string GetSickGatewayIPAddress( ) const;
472 
474  std::string GetSickPartNumber( ) const;
475 
477  std::string GetSickName( ) const;
478 
480  std::string GetSickVersion( ) const;
481 
483  std::string GetSickSerialNumber( ) const;
484 
486  std::string GetSickEDMSerialNumber( ) const;
487 
489  std::string GetSickFirmwarePartNumber( ) const;
490 
492  std::string GetSickFirmwareName( ) const;
493 
495  std::string GetSickFirmwareVersion( ) const;
496 
498  std::string GetSickAppSoftwarePartNumber( ) const;
499 
501  std::string GetSickAppSoftwareName( ) const;
502 
504  std::string GetSickAppSoftwareVersionNumber( ) const;
505 
507  std::string GetSickStatusAsString() const;
508 
510  std::string GetSickIdentityAsString() const;
511 
513  std::string GetSickGlobalConfigAsString() const;
514 
516  std::string GetSickEthernetConfigAsString() const;
517 
519  std::string GetSickSectorConfigAsString() const;
520 
522  double GetSickScanArea( ) const;
523 
525  void PrintSickStatus( ) const;
526 
528  void PrintSickIdentity( ) const;
529 
531  void PrintSickGlobalConfig( ) const;
532 
534  void PrintSickEthernetConfig( ) const;
535 
537  void PrintSickSectorConfig( ) const;
538 
540  void Uninitialize( ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
541 
543  ~SickLD();
544 
545  private:
546 
549 
551  uint16_t _sick_tcp_port;
552 
554  unsigned int _socket;
555 
557  struct sockaddr_in _sick_inet_address_info;
558 
561 
564 
567 
570 
573 
576 
579 
582 
584  void _setupConnection( ) throw( SickIOException, SickTimeoutException );
585 
587  void _syncDriverWithSick( ) throw( SickIOException, SickTimeoutException, SickErrorException );
588 
590  void _setSickSectorFunction( const uint8_t sector_number, const uint8_t sector_function,
591  const double sector_angle_stop, const bool write_to_flash = false )
592  throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
593 
595  void _getSickSectorFunction( const uint8_t sector_num, uint8_t &sector_function, double &sector_stop_angle )
596  throw( SickErrorException, SickTimeoutException, SickIOException );
597 
599  void _setSickSensorModeToIdle( ) throw( SickErrorException, SickTimeoutException, SickIOException );
600 
602  void _setSickSensorModeToRotate( ) throw( SickErrorException, SickTimeoutException, SickIOException );
603 
605  void _setSickSensorModeToMeasure( ) throw( SickErrorException, SickTimeoutException, SickIOException );
606 
608  void _setSickSensorMode( const uint8_t new_sick_sensor_mode )
609  throw( SickErrorException, SickTimeoutException, SickIOException );
610 
612  void _getSickScanProfiles( const uint16_t profile_format, const uint16_t num_profiles = DEFAULT_SICK_NUM_SCAN_PROFILES )
613  throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
614 
616  void _parseScanProfile( uint8_t * const src_buffer, sick_ld_scan_profile_t &profile_data ) const;
617 
619  void _cancelSickScanProfiles( ) throw( SickErrorException, SickTimeoutException, SickIOException );
620 
622  void _setSickFilter( const uint8_t suppress_code )
623  throw( SickErrorException, SickTimeoutException, SickIOException );
624 
626  void _getSickIdentity( ) throw( SickTimeoutException, SickIOException );
627 
629  void _getSickStatus( ) throw( SickTimeoutException, SickIOException );
630 
632  void _setSickGlobalConfig( const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step )
633  throw( SickErrorException, SickTimeoutException, SickIOException );
634 
636  void _getSickGlobalConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
637 
639  void _getSickEthernetConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
640 
642  void _getSickSectorConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
643 
645  void _getIdentificationString( const uint8_t id_request_code, std::string &id_return_string )
646  throw( SickTimeoutException, SickIOException );
647 
649  void _getSensorPartNumber( ) throw( SickTimeoutException, SickIOException );
650 
652  void _getSensorName( ) throw( SickTimeoutException, SickIOException );
653 
655  void _getSensorVersion( ) throw( SickTimeoutException, SickIOException );
656 
658  void _getSensorSerialNumber( ) throw( SickTimeoutException, SickIOException );
659 
661  void _getSensorEDMSerialNumber( ) throw( SickTimeoutException, SickIOException );
662 
664  void _getFirmwarePartNumber( ) throw( SickTimeoutException, SickIOException );
665 
667  void _getFirmwareName( ) throw( SickTimeoutException, SickIOException );
668 
670  void _getFirmwareVersion( ) throw( SickTimeoutException, SickIOException );
671 
673  void _getApplicationSoftwarePartNumber( ) throw( SickTimeoutException, SickIOException );
674 
676  void _getApplicationSoftwareName( ) throw( SickTimeoutException, SickIOException );
677 
679  void _getApplicationSoftwareVersion( ) throw( SickTimeoutException, SickIOException );
680 
682  void _setSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed, const double sick_step_angle,
683  const double * const active_sector_start_angles,
684  const double * const active_sector_stop_angles,
685  const unsigned int num_active_sectors )
686  throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
687 
689  void _setSickTemporaryScanAreas( const double * const active_sector_start_angles,
690  const double * const active_sector_stop_angles,
691  const unsigned int num_active_sectors )
692  throw( SickTimeoutException, SickIOException, SickConfigException );
693 
695  void _setSickSectorConfig( const unsigned int * const sector_functions, const double * const sector_stop_angles,
696  const unsigned int num_sectors, const bool write_to_flash = false )
697  throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
698 
700  void _setSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
701  throw( SickIOException, SickTimeoutException, SickErrorException );
702 
704  void _sendMessageAndGetReply( const SickLDMessage &send_message, SickLDMessage &recv_message,
705  const unsigned int timeout_value = DEFAULT_SICK_MESSAGE_TIMEOUT )
706  throw( SickIOException, SickTimeoutException );
707 
709  void _flushTCPRecvBuffer( ) throw ( SickIOException, SickThreadException );
710 
712  void _teardownConnection( ) throw( SickIOException );
713 
715  void _generateSickSectorConfig( const double * const active_sector_start_angles,
716  const double * const active_sector_stop_angles,
717  const unsigned int num_active_sectors,
718  const double sick_step_angle,
719  unsigned int * const sector_functions,
720  double * const sector_stop_angles,
721  unsigned int &num_sectors ) const;
722 
724  double _ticksToAngle( const uint16_t ticks ) const;
725 
727  uint16_t _angleToTicks( const double angle ) const;
728 
730  double _computeMeanPulseFrequency( const double active_scan_area, const double curr_motor_speed,
731  const double curr_angular_resolution ) const;
732 
734  double _computeMaxPulseFrequency( const double total_scan_area, const double curr_motor_speed,
735  const double curr_angular_resolution ) const;
736 
738  bool _validSickSensorID( const unsigned int sick_sensor_id ) const;
739 
741  bool _validSickMotorSpeed( const unsigned int sick_motor_speed ) const;
742 
744  bool _validSickScanResolution( const double sick_step_angle, const double * const active_sector_start_angles,
745  const double * const active_sector_stop_angles, const unsigned int num_active_sectors ) const;
746 
748  bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle ) const;
749 
751  bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle,
752  const double * const active_sector_start_angles,
753  const double * const active_sector_stop_angles,
754  const unsigned int num_active_sectors ) const;
755 
757  double _computeScanArea( const double sick_step_angle, const double * const sector_start_angles,
758  const double * const sector_stop_angles, const unsigned int num_sectors ) const;
759 
761  void _sortScanAreas( double * const sector_start_angles, double * const sector_stop_angles,
762  const unsigned int num_sectors ) const;
763 
765  bool _validActiveSectors( const double * const sector_start_angles, const double * const sector_stop_angles,
766  const unsigned int num_active_sectors ) const;
767 
769  bool _supportedScanProfileFormat( const uint16_t profile_format ) const;
770 
772  void _printSectorProfileData( const sick_ld_sector_data_t &sector_data ) const;
773 
775  void _printSickScanProfile( const sick_ld_scan_profile_t profile_data, const bool print_sector_data = true ) const;
776 
778  uint8_t _sickSensorModeToWorkServiceSubcode( const uint8_t sick_sensor_mode ) const;
779 
781  std::string _sickSensorModeToString( const uint8_t sick_sensor_mode ) const;
782 
784  std::string _sickMotorModeToString( const uint8_t sick_motor_mode ) const;
785 
787  std::string _sickTransMeasureReturnToString( const uint8_t return_value ) const;
788 
790  std::string _sickResetLevelToString( const uint16_t reset_level ) const;
791 
793  std::string _sickSectorFunctionToString( const uint16_t sick_sector_function ) const;
794 
796  std::string _sickProfileFormatToString( const uint16_t profile_format ) const;
797 
799  void _printInitFooter( ) const;
800 
801  };
802 
803 } //namespace SickToolbox
804 
805 #endif /* SICK_LD_HH */
double _computeMaxPulseFrequency(const double total_scan_area, const double curr_motor_speed, const double curr_angular_resolution) const
Compute the mean pulse frequency (see page 22 of the operator&#39;s manual)
Definition: SickLD.cc:4312
std::string GetSickAppSoftwareName() const
Acquire the Sick LD&#39;s application software name.
Definition: SickLD.cc:1484
uint16_t _sick_tcp_port
Definition: SickLD.hh:551
void _teardownConnection()
Teardown TCP connection to Sick LD.
Definition: SickLD.cc:4168
static const uint8_t SICK_CONF_SECTOR_NO_MEASUREMENT
Sector has no measurements.
Definition: SickLD.hh:182
bool _sick_streaming_range_data
Definition: SickLD.hh:566
double angle_stop
The angle at which the last measurement in the sector was acquired.
Definition: SickLD.hh:343
std::string GetSickIdentityAsString() const
Acquire the Sick LD&#39;s identity as a printable string.
Definition: SickLD.cc:1517
unsigned int timestamp_stop
The timestamp (in ms) corresponding to the time the last measurement in the sector was taken...
Definition: SickLD.hh:339
static const uint8_t SICK_WORK_SERV_TRANS_ROTATE
Sick LD enters ROTATE mode (motor starts and rotates with a specified speed in Hz, laser is off)
Definition: SickLD.hh:142
static const uint8_t SICK_WORK_SERV_RESET
Sick LD enters a reset sequence.
Definition: SickLD.hh:140
void SetSickScanResolution(const double sick_step_angle)
Attempts to set a new scan resolution for the device (in flash) while retaining the previously define...
Definition: SickLD.cc:1102
static const uint8_t SICK_SIGNAL_LED_YELLOW_A
Mask for first yellow LED.
Definition: SickLD.hh:242
static const uint8_t SICK_CONF_SERV_CODE
Configuration service code.
Definition: SickLD.hh:91
static const uint8_t SICK_CONF_SECTOR_RESERVED
Sector is reserved by Sick LD.
Definition: SickLD.hh:183
A structure to aggregate the fields that collectively define the profile of a single scan acquired fr...
Definition: SickLD.hh:358
void _getSickScanProfiles(const uint16_t profile_format, const uint16_t num_profiles=DEFAULT_SICK_NUM_SCAN_PROFILES)
Request n scan profiles from the Sick LD unit.
Definition: SickLD.cc:2389
void _getApplicationSoftwarePartNumber()
Get application software part number.
Definition: SickLD.cc:3691
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM
Request the edm??? serial number.
Definition: SickLD.hh:110
static const uint16_t SICK_MAX_PULSE_FREQUENCY
Max pulse frequency of the device (in Hz) (see page 22 of the operator&#39;s manual)
Definition: SickLD.hh:70
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_VERSION
Request the sensor&#39;s version.
Definition: SickLD.hh:108
static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF
Used to set nearfield suppression off.
Definition: SickLD.hh:132
bool _sick_streaming_range_and_echo_data
Definition: SickLD.hh:569
std::string _sickSensorModeToString(const uint8_t sick_sensor_mode) const
Converts the Sick LD numerical sensor mode to a representative string.
Definition: SickLD.cc:4620
void _sortScanAreas(double *const sector_start_angles, double *const sector_stop_angles, const unsigned int num_sectors) const
Sort the scan areas based on the given angles to place them in device "scan" order.
Definition: SickLD.cc:4472
unsigned int num_data_points
The number of data points in the scan area.
Definition: SickLD.hh:337
uint16_t sick_node_id
Single word address of the Sick LD.
Definition: SickLD.hh:281
static const uint8_t SICK_FILE_SERV_CODE
File service code.
Definition: SickLD.hh:95
void _getApplicationSoftwareVersion()
Get application software part number.
Definition: SickLD.cc:3751
static const uint8_t SICK_CONF_SERV_GET_SYNC_CLOCK
Read the internal time of the LD-OEM/LD-LRS.
Definition: SickLD.hh:123
static const uint8_t SICK_WORK_SERV_TRANS_IDLE
Sick LD enters IDLE mode (motor stops and laser is turned off)
Definition: SickLD.hh:141
uint8_t _sickSensorModeToWorkServiceSubcode(const uint8_t sick_sensor_mode) const
Map Sick LD sensor modes to their equivalent service subcode representations.
Definition: SickLD.cc:4600
static const uint8_t SICK_MONR_SERV_CODE
Monitor service code.
Definition: SickLD.hh:96
void PrintSickGlobalConfig() const
Print the Sick LD&#39;s global configuration.
Definition: SickLD.cc:1638
void _setSickFilter(const uint8_t suppress_code)
Enables/disables nearfield suppression on the Sick LD.
Definition: SickLD.cc:2790
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM
Request the sensor&#39;s part number.
Definition: SickLD.hh:106
static const uint8_t SICK_FILE_SERV_SAVE
Saves the data into flash memory.
Definition: SickLD.hh:166
struct SickToolbox::SickLD::sick_ld_config_ethernet_tag sick_ld_config_ethernet_t
Adopt c-style convention.
static const uint8_t SICK_CONF_SERV_SET_FILTER
Set the filter configuration.
Definition: SickLD.hh:124
Thrown when Sick returns an error code or an unexpected response.
A structure to aggregate the data used to configure the Sick LD global parameter values.
Definition: SickLD.hh:260
static const uint8_t SICK_CONF_KEY_CAN
Key for configuring CAN.
Definition: SickLD.hh:176
Contains some simple exception classes.
static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM
Requess the firmware&#39;s part number.
Definition: SickLD.hh:111
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
Generates a device-ready sector set given only an active sector spec.
Definition: SickLD.cc:4187
double GetSickScanArea() const
Computes the active area over all measuring sectors.
Definition: SickLD.cc:1605
static const uint8_t SICK_FILE_SERV_DELETE
Deletes a file from the flash.
Definition: SickLD.hh:168
std::string sick_application_software_part_number
The Sick LD&#39;s app. software part number.
Definition: SickLD.hh:321
Provides a simple driver interface for working with the Sick LD-OEM/Sick LD-LRS long-range models via...
Definition: SickLD.hh:56
unsigned int timestamp_start
The timestamp (in ms) corresponding to the time the first measurement in the sector was taken...
Definition: SickLD.hh:338
static const uint8_t SICK_CONF_SERV_GET_FUNCTION
Returns the configuration of the given sector.
Definition: SickLD.hh:126
void _getSensorVersion()
Get the Sick LD&#39;s sensor version.
Definition: SickLD.cc:3510
std::string _sickResetLevelToString(const uint16_t reset_level) const
Converts the Sick LD numerical reset level to a representative string.
Definition: SickLD.cc:4690
void SetSickScanAreas(const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors)
Attempts to set the active scan areas for the device (in flash)
Definition: SickLD.cc:1198
void SetSickTimeRelative(const int16_t time_delta, uint16_t &new_sick_clock_time)
Set the relative time of the Sick LD unit.
Definition: SickLD.cc:347
uint8_t _sick_sensor_mode
Definition: SickLD.hh:560
std::string sick_serial_number
The Sick LD&#39;s serial number.
Definition: SickLD.hh:316
double _computeMeanPulseFrequency(const double active_scan_area, const double curr_motor_speed, const double curr_angular_resolution) const
Compute the mean pulse frequency (see page 22 of the operator&#39;s manual)
Definition: SickLD.cc:4299
std::string sick_edm_serial_number
The Sick LD&#39;s edm??? serial number.
Definition: SickLD.hh:317
std::string sick_name
The name assigned to the Sick.
Definition: SickLD.hh:314
void PrintSickSectorConfig() const
Print the Sick LD&#39;s sector configuration.
Definition: SickLD.cc:1652
static const uint8_t SICK_ROUT_SERV_CODE
Routing service code.
Definition: SickLD.hh:94
void GetSickSignals(uint8_t &sick_signal_flags)
Gets the Sick LD signal LED&#39;s and switching outputs.
Definition: SickLD.cc:497
void _setSickSignals(const uint8_t sick_signal_flags=DEFAULT_SICK_SIGNAL_SET)
Sets the Sick LEDs and switching outputs.
Definition: SickLD.cc:4023
static const uint8_t SICK_STAT_SERV_LD_REGISTER_APPLICATION
Registers the ID data for the application firmware.
Definition: SickLD.hh:103
static const uint8_t SICK_CONF_KEY_RS232_RS422
Key for configuring RS-232/RS-422.
Definition: SickLD.hh:175
static const uint8_t SICK_STAT_SERV_CODE
Status service code.
Definition: SickLD.hh:90
static const uint8_t SICK_ROUT_SERV_COM_ATTACH
Attach a master (host) communications interface.
Definition: SickLD.hh:158
unsigned int GetSickSensorID() const
Acquire the Sick LD&#39;s sensor ID.
Definition: SickLD.cc:1331
unsigned int sensor_status
The status of the Sick LD sensor.
Definition: SickLD.hh:362
unsigned int _socket
Definition: SickLD.hh:554
uint8_t sick_num_active_sectors
Number of active sectors (sectors that are actually being scanned)
Definition: SickLD.hh:295
uint8_t sick_num_initialized_sectors
Number of sectors configured w/ a function other than "not initialized".
Definition: SickLD.hh:296
void _getSensorEDMSerialNumber()
Get sensor EDM serial number.
Definition: SickLD.cc:3571
static const uint16_t SICK_MIN_VALID_SENSOR_ID
The lowest value the Sick will accept as a Sensor ID.
Definition: SickLD.hh:67
void DisableNearfieldSuppression()
Disables nearfield suppressive filtering.
Definition: SickLD.cc:668
std::string sick_firmware_version
The Sick LD&#39;s firmware version.
Definition: SickLD.hh:320
uint16_t sick_sensor_id
The single word sensor ID for the Sick unit.
Definition: SickLD.hh:261
static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON
Used to set nearfield suppression on.
Definition: SickLD.hh:133
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE
Sick LD enters MEASURE mode (laser starts with next revolution)
Definition: SickLD.hh:143
void _getSickSectorFunction(const uint8_t sector_num, uint8_t &sector_function, double &sector_stop_angle)
Acquires the function of the given sector.
Definition: SickLD.cc:2016
void _setSickGlobalConfig(const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step)
Sets the Sick LD&#39;s global parameters (sensor id, motor speed, and angular step) in flash...
Definition: SickLD.cc:2988
static const uint8_t SICK_STAT_SERV_GET_ID_APP_PART_NUM
Request the application part number.
Definition: SickLD.hh:114
void EnableNearfieldSuppression()
Enables nearfield suppressive filtering.
Definition: SickLD.cc:619
SickLD(const std::string sick_ip_address=DEFAULT_SICK_IP_ADDRESS, const uint16_t sick_tcp_port=DEFAULT_SICK_TCP_PORT)
A standard constructor.
Definition: SickLD.cc:51
static const uint8_t SICK_MEAS_SERV_GET_PROFILE
Requests n profiles of a defined format.
Definition: SickLD.hh:136
void _setSickSensorModeToRotate()
Sets the Sick LD sensor mode to ROTATE.
Definition: SickLD.cc:2163
static const uint8_t SICK_STAT_SERV_SET_SIGNAL
Sets the switches and LEDs.
Definition: SickLD.hh:102
void _printSickScanProfile(const sick_ld_scan_profile_t profile_data, const bool print_sector_data=true) const
Prints data fields of the given scan profile.
Definition: SickLD.cc:4576
bool _validPulseFrequency(const unsigned int sick_motor_speed, const double sick_step_angle) const
Checks whether the given configuration yields a valid mean and max pulse frequency (uses current sect...
Definition: SickLD.cc:4383
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)
Attempts to set the "permanent" (by writing flash) operating values for the device.
Definition: SickLD.cc:3786
void _sendMessageAndGetReply(const SickLDMessage &send_message, SickLDMessage &recv_message, const unsigned int timeout_value=DEFAULT_SICK_MESSAGE_TIMEOUT)
Send a message to the Sick LD and get its reply.
Definition: SickLD.cc:4083
static const uint8_t SICK_MOTOR_MODE_ERROR
Motor stops or coder error.
Definition: SickLD.hh:86
bool _validSickSensorID(const unsigned int sick_sensor_id) const
Checks whether the given senor id is valid for the device.
Definition: SickLD.cc:4322
static const uint8_t SICK_WORK_SERV_CODE
Working service code.
Definition: SickLD.hh:93
std::string GetSickEDMSerialNumber() const
Acquire the Sick LD&#39;s EDM serial number.
Definition: SickLD.cc:1444
static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD
Code for identifying filter type: nearfield suppression.
Definition: SickLD.hh:129
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM
Request the sensor&#39;s serial number.
Definition: SickLD.hh:109
unsigned int sector_num
The sector number in the scan area.
Definition: SickLD.hh:336
static const uint8_t SICK_ROUT_SERV_COM_INITIALIZE
Initialize the interface (Note: using this may not be necessary for some interfaces, e.g. Ethernet)
Definition: SickLD.hh:160
void _getFirmwareName()
Get firmware name.
Definition: SickLD.cc:3631
A class to represent all messages sent to and from the Sick LD unit.
std::string GetSickSectorConfigAsString() const
Acquire the Sick LD&#39;s sector config as a printable string.
Definition: SickLD.cc:1579
static const uint8_t SICK_FILE_SERV_LOAD
Recalls a file from the flash.
Definition: SickLD.hh:167
bool _validActiveSectors(const double *const sector_start_angles, const double *const sector_stop_angles, const unsigned int num_active_sectors) const
Determines wheter a given set of sector bounds are valid.
Definition: SickLD.cc:4496
sick_ld_config_global_t _sick_global_config
Definition: SickLD.hh:575
std::string sick_application_software_name
The Sick LD&#39;s app. software name.
Definition: SickLD.hh:322
static const uint8_t SICK_MOTOR_MODE_UNKNOWN
Motor is in an unknown state.
Definition: SickLD.hh:87
uint16_t _angleToTicks(const double angle) const
Converts encoder ticks to equivalent angle representation.
Definition: SickLD.cc:4288
uint8_t _sick_motor_mode
Definition: SickLD.hh:563
std::string GetSickGatewayIPAddress() const
Acquire the IP address of the Sick gateway.
Definition: SickLD.cc:1393
static const uint16_t SICK_MAX_MOTOR_SPEED
Maximum motor speed in Hz.
Definition: SickLD.hh:66
void _printSectorProfileData(const sick_ld_sector_data_t &sector_data) const
Print data corresponding to the referenced sector data structure.
Definition: SickLD.cc:4557
void _getFirmwarePartNumber()
Get firmware part number.
Definition: SickLD.cc:3601
void _getIdentificationString(const uint8_t id_request_code, std::string &id_return_string)
Query the Sick LD for a particular ID string.
Definition: SickLD.cc:3395
static const uint16_t SICK_MAX_VALID_SENSOR_ID
The largest value the Sick will accept as a Sensor ID.
Definition: SickLD.hh:68
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)
Sets the sector configuration for the device.
Definition: SickLD.cc:3963
static const uint8_t SICK_SENSOR_MODE_UNKNOWN
The Sick LD is in an unknown state.
Definition: SickLD.hh:80
static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_NAME
Request the firmware&#39;s name.
Definition: SickLD.hh:112
void _flushTCPRecvBuffer()
Flushes TCP receive buffer contents.
Definition: SickLD.cc:4120
static const uint8_t SICK_FILE_SERV_DIR
List the stored files in flash memory.
Definition: SickLD.hh:165
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE
Sick LD reports config yields a max laser pulse frequency that is too high.
Definition: SickLD.hh:152
#define DEFAULT_SICK_NUM_SCAN_PROFILES
Setting this value to 0 will tell the Sick LD to stream measurements when measurement data is request...
Definition: SickLD.hh:25
void _parseScanProfile(uint8_t *const src_buffer, sick_ld_scan_profile_t &profile_data) const
Parses a well-formed sequence of bytes into a corresponding scan profile.
Definition: SickLD.cc:2520
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_OK
Sick LD is ready to stream/obtain scan profiles.
Definition: SickLD.hh:151
void SetSickMotorSpeed(const unsigned int sick_motor_speed)
Attempts to set a new motor speed for the device (in flash)
Definition: SickLD.cc:1040
double _ticksToAngle(const uint16_t ticks) const
Converts encoder ticks to equivalent angle representation.
Definition: SickLD.cc:4275
std::string GetSickFirmwareVersion() const
Acquire the Sick LD&#39;s firmware version.
Definition: SickLD.cc:1468
static const uint8_t SICK_STAT_SERV_GET_SIGNAL
Reads the value of the switch and LED port.
Definition: SickLD.hh:101
std::string GetSickSubnetMask() const
Acquire the subnet mask for the Sick.
Definition: SickLD.cc:1374
A structure to aggregate the fields that collectively define a sector in the scan area of the Sick LD...
Definition: SickLD.hh:335
static const uint16_t SICK_MIN_MOTOR_SPEED
Minimum motor speed in Hz.
Definition: SickLD.hh:65
static const uint8_t SICK_CONF_SECTOR_NORMAL_MEASUREMENT
Sector is returning measurements.
Definition: SickLD.hh:184
static const uint8_t SICK_MEAS_SERV_CANCEL_PROFILE
Stops profile output.
Definition: SickLD.hh:137
static const uint8_t SICK_CONF_KEY_ETHERNET
Key for configuring Ethernet.
Definition: SickLD.hh:177
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
Checks whether the given scan resolution is valid.
Definition: SickLD.cc:4354
double sick_angle_step
Difference between two laser pulse positions in 1/16th deg. (NOTE: this value must be a divisor of 57...
Definition: SickLD.hh:263
std::string GetSickName() const
Acquire the Sick LD&#39;s name.
Definition: SickLD.cc:1420
std::string sick_part_number
The Sick LD&#39;s part number.
Definition: SickLD.hh:313
struct sockaddr_in _sick_inet_address_info
Definition: SickLD.hh:557
static const uint8_t SICK_ROUT_SERV_COM_DETACH
Detach a master (host) communications interface.
Definition: SickLD.hh:159
std::string _sick_ip_address
Definition: SickLD.hh:548
static const uint8_t SICK_CONF_SECTOR_REFERENCE_MEASUREMENT
Sector can be used as reference measurement.
Definition: SickLD.hh:185
uint16_t sick_motor_speed
Nominal motor speed value: 0x0005 to 0x0014 (5 to 20)
Definition: SickLD.hh:262
static const uint8_t SICK_SIGNAL_LED_RED
Mask for red LED.
Definition: SickLD.hh:245
std::string GetSickFirmwareName() const
Acquire the Sick LD&#39;s firmware number.
Definition: SickLD.cc:1460
void _getSickIdentity()
Get the parameters that define the Sick LD&#39;s identity.
Definition: SickLD.cc:2885
static const uint8_t SICK_SIGNAL_SWITCH_3
Mask for signal switch 3.
Definition: SickLD.hh:249
void _printInitFooter() const
Prints the initialization footer.
Definition: SickLD.cc:4750
static const uint8_t SICK_MEAS_SERV_CODE
Measurement service code.
Definition: SickLD.hh:92
static const uint8_t SICK_MONR_SERV_MONITOR_PROFILE_LOG
Enable/disable profile logging.
Definition: SickLD.hh:172
static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_LOW
Motor spin too high (i.e. rotational velocity too fast)
Definition: SickLD.hh:85
static const uint8_t SICK_STAT_SERV_GET_ID
Request the Sick LD ID.
Definition: SickLD.hh:99
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)
Attempts to set the scan resolution and active sectors/scan areas for the device (in flash) ...
Definition: SickLD.cc:1140
struct SickToolbox::SickLD::sick_ld_config_global_tag sick_ld_config_global_t
Adopt c-style convention.
void _getSickGlobalConfig()
Get the global configuration of the Sick LD.
Definition: SickLD.cc:3088
static const uint8_t SICK_SENSOR_MODE_MEASURE
The Sick LD prism is rotating, and the laser is on.
Definition: SickLD.hh:78
Provides an abstract parent for all Sick LIDAR devices.
Definition: SickLIDAR.hh:53
static const uint8_t SICK_SENSOR_MODE_ROTATE
The Sick LD prism is rotating, but laser is off.
Definition: SickLD.hh:77
std::string sick_firmware_name
The Sick LD&#39;s firmware name.
Definition: SickLD.hh:319
static const uint8_t SICK_SIGNAL_SWITCH_1
Mask for signal switch 1.
Definition: SickLD.hh:247
void ResetSick(const unsigned int reset_level=SICK_WORK_SERV_RESET_INIT_CPU)
Resets the device according to the given reset level.
Definition: SickLD.cc:1252
#define DEFAULT_SICK_IP_ADDRESS
Default Sick LD INet 4 address.
Definition: SickLD.hh:21
std::string GetSickEthernetConfigAsString() const
Acquire the Sick LD&#39;s global config as a printable string.
Definition: SickLD.cc:1561
void GetSickTime(uint16_t &sick_time)
Gets the internal clock time of the Sick LD unit.
Definition: SickLD.cc:557
void _setSickSectorFunction(const uint8_t sector_number, const uint8_t sector_function, const double sector_angle_stop, const bool write_to_flash=false)
Sets the function for a particular scan sector.
Definition: SickLD.cc:1890
sick_ld_config_ethernet_t _sick_ethernet_config
Definition: SickLD.hh:578
static constexpr double SICK_DEGREES_PER_MOTOR_STEP
Each odometer tick is equivalent to rotating the scan head this many degrees.
Definition: SickLD.hh:73
static const uint8_t SICK_MOTOR_MODE_OK
Motor is functioning properly.
Definition: SickLD.hh:83
static const uint8_t SICK_WORK_SERV_RESET_INIT_CPU
Sick LD does a complete reset (Reinitializes the CPU)
Definition: SickLD.hh:146
std::string sick_version
The Sick LD&#39;s version number.
Definition: SickLD.hh:315
std::string GetSickAppSoftwareVersionNumber() const
Acquire the Sick LD&#39;s application software version number.
Definition: SickLD.cc:1492
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
Computes the active scan area for the Sick given the current sector configuration.
Definition: SickLD.cc:4444
void _setSickSensorModeToIdle()
Sets the Sick LD sensor mode to IDLE.
Definition: SickLD.cc:2120
static const uint8_t SICK_MONR_SERV_MONITOR_RUN
Enable/disable monitor services.
Definition: SickLD.hh:171
void _cancelSickScanProfiles()
Kills the current data stream.
Definition: SickLD.cc:2687
unsigned int motor_status
The status of the Sick LD motor.
Definition: SickLD.hh:363
unsigned int profile_number
The number of profiles sent to the host (i.e. the current profile number)
Definition: SickLD.hh:359
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE
Sick LD reports config yields a max mean pulse frequency that is too high.
Definition: SickLD.hh:153
void PrintSickStatus() const
Print the status of the Sick LD.
Definition: SickLD.cc:1624
static const uint8_t SICK_SIGNAL_SWITCH_0
Mask for signal switch 0.
Definition: SickLD.hh:246
void _getSensorSerialNumber()
Get the Sick LD&#39;s serial number.
Definition: SickLD.cc:3541
static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION
Request the firmware&#39;s version.
Definition: SickLD.hh:113
std::string GetSickPartNumber() const
Acquire the Sick LD&#39;s part number.
Definition: SickLD.cc:1412
Defines the abstract parent class for defining a Sick LIDAR device driver.
std::string _sickProfileFormatToString(const uint16_t profile_format) const
Converts the Sick LD numerical motor mode to a representative string.
Definition: SickLD.cc:4734
A structure to aggregate the fields that collectively define the identity of a Sick LD unit...
Definition: SickLD.hh:312
void Initialize()
Initializes the driver and syncs it with Sick LD unit. Uses sector config given in flash...
Definition: SickLD.cc:91
void _setSickTemporaryScanAreas(const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors)
Attempts to set the "temporary" (until a device reset) scan area config for the device.
Definition: SickLD.cc:3885
void SetSickSensorID(const unsigned int sick_sensor_id)
Attempts to set a new sensor ID for the device (in flash)
Definition: SickLD.cc:991
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)
Acquires measurements and corresponding sector data from the Sick LD.
Definition: SickLD.cc:740
static const uint8_t SICK_STAT_SERV_GET_ID_APP_VERSION
Request the application version.
Definition: SickLD.hh:116
static const uint8_t SICK_SENSOR_MODE_ERROR
The Sick LD is in error mode.
Definition: SickLD.hh:79
static constexpr double SICK_MAX_SCAN_ANGULAR_RESOLUTION
Minimum valid separation between laser pulses in active scan ares (deg)
Definition: SickLD.hh:72
static const uint8_t SICK_CONF_SERV_SET_CONFIGURATION
Set the Sick LD configuration.
Definition: SickLD.hh:119
static const uint16_t SICK_MAX_NUM_SECTORS
Maximum number of scan sectors (NOTE: This value must be even)
Definition: SickLD.hh:62
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT
Sick LD reports sector borders are not a multiple of the step angle.
Definition: SickLD.hh:155
Defines a class for monitoring the receive buffer when interfacing w/ a Sick LMS LIDAR.
static const uint16_t SICK_MAX_MEAN_PULSE_FREQUENCY
Max mean pulse frequence of the current device configuration (in Hz) (see page 22 of the operator&#39;s m...
Definition: SickLD.hh:69
A structure to aggregate the data used to configure the Sick LD unit for Ethernet.
Definition: SickLD.hh:277
void PrintSickEthernetConfig() const
Print the Sick LD&#39;s Ethernet configuration.
Definition: SickLD.cc:1645
static const uint8_t SICK_SENSOR_MODE_IDLE
The Sick LD is powered but idle.
Definition: SickLD.hh:76
static const uint8_t SICK_SIGNAL_LED_YELLOW_B
Mask for second yellow LED.
Definition: SickLD.hh:243
struct SickToolbox::SickLD::sick_ld_config_sector_tag sick_ld_config_sector_t
Adopt c-style convention.
void SetSickSignals(const uint8_t sick_signal_flags=DEFAULT_SICK_SIGNAL_SET)
Sets the Sick LD signal LED&#39;s and switching outputs.
Definition: SickLD.cc:453
static const uint16_t SICK_MAX_NUM_MEASUREMENTS
Maximum number of measurements per sector.
Definition: SickLD.hh:61
unsigned int GetSickMotorSpeed() const
Acquire the Sick LD&#39;s current motor speed in Hz.
Definition: SickLD.cc:1339
void _getSickSectorConfig()
Query the Sick for its current sector configuration.
Definition: SickLD.cc:3318
Encapsulates the Sick LIDAR Matlab/C++ toolbox.
Definition: SickLD.cc:44
static const uint16_t SICK_SCAN_PROFILE_RANGE_AND_ECHO
Request sector scan data w/ echo data.
Definition: SickLD.hh:212
unsigned int GetSickNumActiveSectors() const
Acquire the number of sectors that are measuring.
Definition: SickLD.cc:1323
std::string GetSickIPAddress() const
Acquire the current IP address of the Sick.
Definition: SickLD.cc:1355
void _getSensorPartNumber()
Get the Sick LD&#39;s part number.
Definition: SickLD.cc:3448
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_NAME
Request the sensor&#39;s name.
Definition: SickLD.hh:107
unsigned int profile_counter
The number of profiles gathered by the Sick LD.
Definition: SickLD.hh:360
void _setSickSensorModeToMeasure()
Sets the Sick LD sensor mode to ROTATE.
Definition: SickLD.cc:2206
#define DEFAULT_SICK_TCP_PORT
Default TCP port.
Definition: SickLD.hh:22
std::string _sickMotorModeToString(const uint8_t sick_motor_mode) const
Converts the Sick LD numerical motor mode to a representative string.
Definition: SickLD.cc:4643
Defines the class SickLDMessage.
void _setSickSensorMode(const uint8_t new_sick_sensor_mode)
Sets the Sick LD to the requested sensor mode.
Definition: SickLD.cc:2250
static const uint8_t SICK_ROUT_SERV_COM_DATA
Forward data received on specified interface to master interface.
Definition: SickLD.hh:162
static const uint8_t SICK_WORK_SERV_RESET_HALT_APP
Sick LD does a minimal reset (Application is halted and device enters IDLE state) ...
Definition: SickLD.hh:148
#define DEFAULT_SICK_SIGNAL_SET
Default Sick signal configuration.
Definition: SickLD.hh:26
static const uint8_t SICK_CONF_SERV_GET_CONFIGURATION
Read the Sick LD configuration information.
Definition: SickLD.hh:120
uint16_t sick_transparent_tcp_port
The TCP/IP transparent port associated with the Sick LD.
Definition: SickLD.hh:282
struct SickToolbox::SickLD::sick_ld_sector_data_tag sick_ld_sector_data_t
Adopt c-style convention.
static const uint16_t SICK_SCAN_PROFILE_RANGE
Request sector scan data w/o any echo data.
Definition: SickLD.hh:188
Thrown instance where the driver can&#39;t read,write,drain,flush,... the buffers.
void PrintSickIdentity() const
Print the parameters comprising the Sick LD&#39;s identity.
Definition: SickLD.cc:1631
void Uninitialize()
Tear down the connection between the host and the Sick LD.
Definition: SickLD.cc:1659
sick_ld_config_sector_t _sick_sector_config
Definition: SickLD.hh:581
double angle_step
The angle step used for the given sector (this should be the same for all sectors) ...
Definition: SickLD.hh:341
std::string GetSickSerialNumber() const
Acquire the Sick LD&#39;s serial number.
Definition: SickLD.cc:1436
bool _validSickMotorSpeed(const unsigned int sick_motor_speed) const
Checks whether the given sick motor speed is valid for the device.
Definition: SickLD.cc:4337
std::string GetSickVersion() const
Acquire the Sick LD&#39;s version number.
Definition: SickLD.cc:1428
void _getFirmwareVersion()
Get firmware version number.
Definition: SickLD.cc:3661
struct SickToolbox::SickLD::sick_ld_scan_profile_tag sick_ld_scan_profile_t
Adopt c-style convention.
std::string GetSickFirmwarePartNumber() const
Acquire the Sick LD&#39;s firmware part number.
Definition: SickLD.cc:1452
unsigned int num_sectors
The number of sectors returned in the profile.
Definition: SickLD.hh:364
static const uint8_t SICK_SIGNAL_SWITCH_2
Mask for signal switch 2.
Definition: SickLD.hh:248
void _getApplicationSoftwareName()
Get application software name.
Definition: SickLD.cc:3721
std::string GetSickAppSoftwarePartNumber() const
Acquire the Sick LD&#39;s application software part number.
Definition: SickLD.cc:1476
static const uint8_t SICK_ROUT_SERV_COM_OUTPUT
Output data to the interface.
Definition: SickLD.hh:161
std::string _sickSectorFunctionToString(const uint16_t sick_sector_function) const
Converts the Sick LD numerical sector config to a representative string.
Definition: SickLD.cc:4710
double angle_start
The angle at which the first measurement in the sector was acquired.
Definition: SickLD.hh:342
std::string sick_application_software_version
The Sick LD&#39;s app. software version.
Definition: SickLD.hh:323
static const uint8_t SICK_CONF_SECTOR_NOT_INITIALIZED
Sector is uninitialized.
Definition: SickLD.hh:181
void SetSickTimeAbsolute(const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time)
Set the absolute time of the Sick LD unit.
Definition: SickLD.cc:241
void _syncDriverWithSick()
Synchronizes buffer driver parameters with those of the Sick LD.
Definition: SickLD.cc:1841
static const uint8_t SICK_WORK_SERV_RESET_KEEP_CPU
Sick LD does a partial reset (CPU is not reinitialized)
Definition: SickLD.hh:147
#define DEFAULT_SICK_MESSAGE_TIMEOUT
The max time to wait for a message reply (usecs)
Definition: SickLD.hh:23
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER
Sick LD reports sector borders are not configured correctly.
Definition: SickLD.hh:154
void _getSickStatus()
Get the status of the Sick LD.
Definition: SickLD.cc:2927
static const uint16_t SICK_NUM_TICKS_PER_MOTOR_REV
Odometer ticks per revolution of the Sick LD scan head.
Definition: SickLD.hh:71
Thrown when the driver detects (or the Sick reports) an invalid config.
static const uint16_t SICK_MAX_NUM_MEASURING_SECTORS
Maximum number of active/measuring scan sectors.
Definition: SickLD.hh:63
static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_HIGH
Motor spin too low (i.e. rotational velocity too low)
Definition: SickLD.hh:84
static const uint8_t SICK_CONF_SERV_SET_TIME_RELATIVE
Correct the internal clock by some value.
Definition: SickLD.hh:122
void _setupConnection()
Establish a TCP connection to the unit.
Definition: SickLD.cc:1732
static const uint8_t SICK_STAT_SERV_GET_ID_APP_NAME
Request the application name.
Definition: SickLD.hh:115
static const uint16_t SICK_MAX_SCAN_AREA
Maximum area that can be covered in a single scan (deg)
Definition: SickLD.hh:64
Makes handling timeouts much easier.
static const uint8_t SICK_SIGNAL_LED_GREEN
Mask for green LED.
Definition: SickLD.hh:244
struct SickToolbox::SickLD::sick_ld_identity_tag sick_ld_identity_t
Adopt c-style convention.
unsigned int layer_num
The layer number associated with a scan (this will always be 0)
Definition: SickLD.hh:361
void _getSensorName()
Get the Sick LD&#39;s assigned sensor name.
Definition: SickLD.cc:3479
A structure to aggregate data used to define the Sick LD&#39;s sector configuration.
Definition: SickLD.hh:294
sick_ld_identity_t _sick_identity
Definition: SickLD.hh:572
std::string sick_firmware_part_number
The Sick LD&#39;s firmware part number.
Definition: SickLD.hh:318
void SetSickTempScanAreas(const double *active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors)
Set the temporary scan areas for the Sick LD.
Definition: SickLD.cc:190
static const uint8_t SICK_CONF_KEY_GLOBAL
Key for global configuration.
Definition: SickLD.hh:178
double GetSickScanResolution() const
Acquire the Sick LD&#39;s current scan resolution.
Definition: SickLD.cc:1347
std::string GetSickGlobalConfigAsString() const
Acquire the Sick LD&#39;s global config as a printable string.
Definition: SickLD.cc:1543
std::string _sickTransMeasureReturnToString(const uint8_t return_value) const
Converts return value from TRANS_MEASURE to a representative string.
Definition: SickLD.cc:4666
Thrown when error occurs during thread initialization, and uninitialization.
std::string GetSickStatusAsString() const
Acquire the Sick LD&#39;s status as a printable string.
Definition: SickLD.cc:1500
void GetSickStatus(unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode)
Acquires the status of the Sick from the device.
Definition: SickLD.cc:146
static const uint8_t SICK_CONF_SERV_SET_TIME_ABSOLUTE
Set the internal clock to a timestamp value.
Definition: SickLD.hh:121
bool _supportedScanProfileFormat(const uint16_t profile_format) const
Check that the given profile format is supported by the current driver version.
Definition: SickLD.cc:4540
void _getSickEthernetConfig()
Get the Sick LD&#39;s Ethernet configuration.
Definition: SickLD.cc:3192
static const uint8_t SICK_STAT_SERV_GET_STATUS
Request status information.
Definition: SickLD.hh:100
static const uint8_t SICK_CONF_SERV_SET_FUNCTION
Assigns a measurement function to an angle range.
Definition: SickLD.hh:125


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Tue Sep 10 2019 03:37:34