Provides a simple driver interface for working with the Sick LD-OEM/Sick LD-LRS long-range models via Ethernet. More...
#include <SickLD.hh>
Classes | |
struct | sick_ld_config_ethernet_tag |
A structure to aggregate the data used to configure the Sick LD unit for Ethernet. More... | |
struct | sick_ld_config_global_tag |
A structure to aggregate the data used to configure the Sick LD global parameter values. More... | |
struct | sick_ld_config_sector_tag |
A structure to aggregate data used to define the Sick LD's sector configuration. More... | |
struct | sick_ld_identity_tag |
A structure to aggregate the fields that collectively define the identity of a Sick LD unit. More... | |
struct | sick_ld_scan_profile_tag |
A structure to aggregate the fields that collectively define the profile of a single scan acquired from the Sick LD unit. More... | |
struct | sick_ld_sector_data_tag |
A structure to aggregate the fields that collectively define a sector in the scan area of the Sick LD unit. More... | |
Public Types | |
typedef struct SickToolbox::SickLD::sick_ld_config_ethernet_tag | sick_ld_config_ethernet_t |
Adopt c-style convention. More... | |
typedef struct SickToolbox::SickLD::sick_ld_config_global_tag | sick_ld_config_global_t |
Adopt c-style convention. More... | |
typedef struct SickToolbox::SickLD::sick_ld_config_sector_tag | sick_ld_config_sector_t |
Adopt c-style convention. More... | |
typedef struct SickToolbox::SickLD::sick_ld_identity_tag | sick_ld_identity_t |
Adopt c-style convention. More... | |
typedef struct SickToolbox::SickLD::sick_ld_scan_profile_tag | sick_ld_scan_profile_t |
Adopt c-style convention. More... | |
typedef struct SickToolbox::SickLD::sick_ld_sector_data_tag | sick_ld_sector_data_t |
Adopt c-style convention. More... | |
Public Member Functions | |
void | DisableNearfieldSuppression () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Disables nearfield suppressive filtering. More... | |
void | EnableNearfieldSuppression () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Enables nearfield suppressive filtering. More... | |
std::string | GetSickAppSoftwareName () const |
Acquire the Sick LD's application software name. More... | |
std::string | GetSickAppSoftwarePartNumber () const |
Acquire the Sick LD's application software part number. More... | |
std::string | GetSickAppSoftwareVersionNumber () const |
Acquire the Sick LD's application software version number. More... | |
std::string | GetSickEDMSerialNumber () const |
Acquire the Sick LD's EDM serial number. More... | |
std::string | GetSickEthernetConfigAsString () const |
Acquire the Sick LD's global config as a printable string. More... | |
std::string | GetSickFirmwareName () const |
Acquire the Sick LD's firmware number. More... | |
std::string | GetSickFirmwarePartNumber () const |
Acquire the Sick LD's firmware part number. More... | |
std::string | GetSickFirmwareVersion () const |
Acquire the Sick LD's firmware version. More... | |
std::string | GetSickGatewayIPAddress () const |
Acquire the IP address of the Sick gateway. More... | |
std::string | GetSickGlobalConfigAsString () const |
Acquire the Sick LD's global config as a printable string. More... | |
std::string | GetSickIdentityAsString () const |
Acquire the Sick LD's identity as a printable string. More... | |
std::string | GetSickIPAddress () const |
Acquire the current IP address of the Sick. More... | |
void | GetSickMeasurements (double *const range_measurements, unsigned int *const echo_measurements=NULL, unsigned int *const num_measurements=NULL, unsigned int *const sector_ids=NULL, unsigned int *const sector_data_offsets=NULL, double *const sector_step_angles=NULL, double *const sector_start_angles=NULL, double *const sector_stop_angles=NULL, unsigned int *const sector_start_timestamps=NULL, unsigned int *const sector_stop_timestamps=NULL) throw ( SickErrorException, SickIOException, SickTimeoutException, SickConfigException ) |
Acquires measurements and corresponding sector data from the Sick LD. More... | |
unsigned int | GetSickMotorSpeed () const |
Acquire the Sick LD's current motor speed in Hz. More... | |
std::string | GetSickName () const |
Acquire the Sick LD's name. More... | |
unsigned int | GetSickNumActiveSectors () const |
Acquire the number of sectors that are measuring. More... | |
std::string | GetSickPartNumber () const |
Acquire the Sick LD's part number. More... | |
double | GetSickScanArea () const |
Computes the active area over all measuring sectors. More... | |
double | GetSickScanResolution () const |
Acquire the Sick LD's current scan resolution. More... | |
std::string | GetSickSectorConfigAsString () const |
Acquire the Sick LD's sector config as a printable string. More... | |
unsigned int | GetSickSensorID () const |
Acquire the Sick LD's sensor ID. More... | |
std::string | GetSickSerialNumber () const |
Acquire the Sick LD's serial number. More... | |
void | GetSickSignals (uint8_t &sick_signal_flags) throw ( SickIOException, SickTimeoutException ) |
Gets the Sick LD signal LED's and switching outputs. More... | |
void | GetSickStatus (unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode) throw ( SickIOException, SickTimeoutException ) |
Acquires the status of the Sick from the device. More... | |
std::string | GetSickStatusAsString () const |
Acquire the Sick LD's status as a printable string. More... | |
std::string | GetSickSubnetMask () const |
Acquire the subnet mask for the Sick. More... | |
void | GetSickTime (uint16_t &sick_time) throw ( SickIOException, SickTimeoutException, SickErrorException ) |
Gets the internal clock time of the Sick LD unit. More... | |
std::string | GetSickVersion () const |
Acquire the Sick LD's version number. More... | |
void | Initialize () throw ( SickIOException, SickThreadException, SickTimeoutException, SickErrorException ) |
Initializes the driver and syncs it with Sick LD unit. Uses sector config given in flash. More... | |
void | PrintSickEthernetConfig () const |
Print the Sick LD's Ethernet configuration. More... | |
void | PrintSickGlobalConfig () const |
Print the Sick LD's global configuration. More... | |
void | PrintSickIdentity () const |
Print the parameters comprising the Sick LD's identity. More... | |
void | PrintSickSectorConfig () const |
Print the Sick LD's sector configuration. More... | |
void | PrintSickStatus () const |
Print the status of the Sick LD. More... | |
void | ResetSick (const unsigned int reset_level=SICK_WORK_SERV_RESET_INIT_CPU) throw ( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) |
Resets the device according to the given reset level. More... | |
void | SetSickGlobalParamsAndScanAreas (const unsigned int sick_motor_speed, const double sick_step_angle, const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors) throw ( SickTimeoutException, SickIOException, SickConfigException, SickErrorException ) |
Attempts to set the scan resolution and active sectors/scan areas for the device (in flash) More... | |
void | SetSickMotorSpeed (const unsigned int sick_motor_speed) throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Attempts to set a new motor speed for the device (in flash) More... | |
void | SetSickScanAreas (const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors) throw ( SickTimeoutException, SickIOException, SickConfigException, SickErrorException ) |
Attempts to set the active scan areas for the device (in flash) More... | |
void | SetSickScanResolution (const double sick_step_angle) throw ( SickTimeoutException, SickIOException, SickConfigException ) |
Attempts to set a new scan resolution for the device (in flash) while retaining the previously defined active scan areas. More... | |
void | SetSickSensorID (const unsigned int sick_sensor_id) throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Attempts to set a new sensor ID for the device (in flash) More... | |
void | SetSickSignals (const uint8_t sick_signal_flags=DEFAULT_SICK_SIGNAL_SET) throw ( SickIOException, SickTimeoutException, SickErrorException ) |
Sets the Sick LD signal LED's and switching outputs. More... | |
void | SetSickTempScanAreas (const double *active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors) throw ( SickTimeoutException, SickIOException, SickConfigException ) |
Set the temporary scan areas for the Sick LD. More... | |
void | SetSickTimeAbsolute (const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time) throw ( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) |
Set the absolute time of the Sick LD unit. More... | |
void | SetSickTimeRelative (const int16_t time_delta, uint16_t &new_sick_clock_time) throw ( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) |
Set the relative time of the Sick LD unit. More... | |
SickLD (const std::string sick_ip_address=DEFAULT_SICK_IP_ADDRESS, const uint16_t sick_tcp_port=DEFAULT_SICK_TCP_PORT) | |
A standard constructor. More... | |
void | Uninitialize () throw ( SickIOException, SickTimeoutException, SickErrorException, SickThreadException ) |
Tear down the connection between the host and the Sick LD. More... | |
~SickLD () | |
Public Member Functions inherited from SickToolbox::SickLIDAR< SickLDBufferMonitor, SickLDMessage > | |
bool | IsInitialized () |
SickLIDAR () | |
Initializes the buffer monitor. More... | |
virtual | ~SickLIDAR () |
Destructor tears down buffer monitor. More... | |
Static Public Attributes | |
static const uint8_t | SICK_CONF_KEY_CAN = 0x02 |
Key for configuring CAN. More... | |
static const uint8_t | SICK_CONF_KEY_ETHERNET = 0x05 |
Key for configuring Ethernet. More... | |
static const uint8_t | SICK_CONF_KEY_GLOBAL = 0x10 |
Key for global configuration. More... | |
static const uint8_t | SICK_CONF_KEY_RS232_RS422 = 0x01 |
Key for configuring RS-232/RS-422. More... | |
static const uint8_t | SICK_CONF_SECTOR_NO_MEASUREMENT = 0x01 |
Sector has no measurements. More... | |
static const uint8_t | SICK_CONF_SECTOR_NORMAL_MEASUREMENT = 0x03 |
Sector is returning measurements. More... | |
static const uint8_t | SICK_CONF_SECTOR_NOT_INITIALIZED = 0x00 |
Sector is uninitialized. More... | |
static const uint8_t | SICK_CONF_SECTOR_REFERENCE_MEASUREMENT = 0x04 |
Sector can be used as reference measurement. More... | |
static const uint8_t | SICK_CONF_SECTOR_RESERVED = 0x02 |
Sector is reserved by Sick LD. More... | |
static const uint8_t | SICK_CONF_SERV_CODE = 0x02 |
Configuration service code. More... | |
static const uint8_t | SICK_CONF_SERV_GET_CONFIGURATION = 0x02 |
Read the Sick LD configuration information. More... | |
static const uint8_t | SICK_CONF_SERV_GET_FUNCTION = 0x0B |
Returns the configuration of the given sector. More... | |
static const uint8_t | SICK_CONF_SERV_GET_SYNC_CLOCK = 0x05 |
Read the internal time of the LD-OEM/LD-LRS. More... | |
static const uint8_t | SICK_CONF_SERV_SET_CONFIGURATION = 0x01 |
Set the Sick LD configuration. More... | |
static const uint8_t | SICK_CONF_SERV_SET_FILTER = 0x09 |
Set the filter configuration. More... | |
static const uint8_t | SICK_CONF_SERV_SET_FILTER_NEARFIELD = 0x01 |
Code for identifying filter type: nearfield suppression. More... | |
static const uint8_t | SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF = 0x00 |
Used to set nearfield suppression off. More... | |
static const uint8_t | SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON = 0x01 |
Used to set nearfield suppression on. More... | |
static const uint8_t | SICK_CONF_SERV_SET_FUNCTION = 0x0A |
Assigns a measurement function to an angle range. More... | |
static const uint8_t | SICK_CONF_SERV_SET_TIME_ABSOLUTE = 0x03 |
Set the internal clock to a timestamp value. More... | |
static const uint8_t | SICK_CONF_SERV_SET_TIME_RELATIVE = 0x04 |
Correct the internal clock by some value. More... | |
static constexpr double | SICK_DEGREES_PER_MOTOR_STEP = 0.0625 |
Each odometer tick is equivalent to rotating the scan head this many degrees. More... | |
static const uint8_t | SICK_FILE_SERV_CODE = 0x07 |
File service code. More... | |
static const uint8_t | SICK_FILE_SERV_DELETE = 0x04 |
Deletes a file from the flash. More... | |
static const uint8_t | SICK_FILE_SERV_DIR = 0x01 |
List the stored files in flash memory. More... | |
static const uint8_t | SICK_FILE_SERV_LOAD = 0x03 |
Recalls a file from the flash. More... | |
static const uint8_t | SICK_FILE_SERV_SAVE = 0x02 |
Saves the data into flash memory. More... | |
static const uint16_t | SICK_MAX_MEAN_PULSE_FREQUENCY = 10800 |
Max mean pulse frequence of the current device configuration (in Hz) (see page 22 of the operator's manual) More... | |
static const uint16_t | SICK_MAX_MOTOR_SPEED = 20 |
Maximum motor speed in Hz. More... | |
static const uint16_t | SICK_MAX_NUM_MEASUREMENTS = 2881 |
Maximum number of measurements per sector. More... | |
static const uint16_t | SICK_MAX_NUM_MEASURING_SECTORS = 4 |
Maximum number of active/measuring scan sectors. More... | |
static const uint16_t | SICK_MAX_NUM_SECTORS = 8 |
Maximum number of scan sectors (NOTE: This value must be even) More... | |
static const uint16_t | SICK_MAX_PULSE_FREQUENCY = 14400 |
Max pulse frequency of the device (in Hz) (see page 22 of the operator's manual) More... | |
static constexpr double | SICK_MAX_SCAN_ANGULAR_RESOLUTION = 0.125 |
Minimum valid separation between laser pulses in active scan ares (deg) More... | |
static const uint16_t | SICK_MAX_SCAN_AREA = 360 |
Maximum area that can be covered in a single scan (deg) More... | |
static const uint16_t | SICK_MAX_VALID_SENSOR_ID = 254 |
The largest value the Sick will accept as a Sensor ID. More... | |
static const uint8_t | SICK_MEAS_SERV_CANCEL_PROFILE = 0x02 |
Stops profile output. More... | |
static const uint8_t | SICK_MEAS_SERV_CODE = 0x03 |
Measurement service code. More... | |
static const uint8_t | SICK_MEAS_SERV_GET_PROFILE = 0x01 |
Requests n profiles of a defined format. More... | |
static const uint16_t | SICK_MIN_MOTOR_SPEED = 5 |
Minimum motor speed in Hz. More... | |
static const uint16_t | SICK_MIN_VALID_SENSOR_ID = 1 |
The lowest value the Sick will accept as a Sensor ID. More... | |
static const uint8_t | SICK_MONR_SERV_CODE = 0x08 |
Monitor service code. More... | |
static const uint8_t | SICK_MONR_SERV_MONITOR_PROFILE_LOG = 0x02 |
Enable/disable profile logging. More... | |
static const uint8_t | SICK_MONR_SERV_MONITOR_RUN = 0x01 |
Enable/disable monitor services. More... | |
static const uint8_t | SICK_MOTOR_MODE_ERROR = 0x0B |
Motor stops or coder error. More... | |
static const uint8_t | SICK_MOTOR_MODE_OK = 0x00 |
Motor is functioning properly. More... | |
static const uint8_t | SICK_MOTOR_MODE_SPIN_TOO_HIGH = 0x09 |
Motor spin too low (i.e. rotational velocity too low) More... | |
static const uint8_t | SICK_MOTOR_MODE_SPIN_TOO_LOW = 0x04 |
Motor spin too high (i.e. rotational velocity too fast) More... | |
static const uint8_t | SICK_MOTOR_MODE_UNKNOWN = 0xFF |
Motor is in an unknown state. More... | |
static const uint16_t | SICK_NUM_TICKS_PER_MOTOR_REV = 5760 |
Odometer ticks per revolution of the Sick LD scan head. More... | |
static const uint8_t | SICK_ROUT_SERV_CODE = 0x06 |
Routing service code. More... | |
static const uint8_t | SICK_ROUT_SERV_COM_ATTACH = 0x01 |
Attach a master (host) communications interface. More... | |
static const uint8_t | SICK_ROUT_SERV_COM_DATA = 0x05 |
Forward data received on specified interface to master interface. More... | |
static const uint8_t | SICK_ROUT_SERV_COM_DETACH = 0x02 |
Detach a master (host) communications interface. More... | |
static const uint8_t | SICK_ROUT_SERV_COM_INITIALIZE = 0x03 |
Initialize the interface (Note: using this may not be necessary for some interfaces, e.g. Ethernet) More... | |
static const uint8_t | SICK_ROUT_SERV_COM_OUTPUT = 0x04 |
Output data to the interface. More... | |
static const uint16_t | SICK_SCAN_PROFILE_RANGE = 0x39FF |
Request sector scan data w/o any echo data. More... | |
static const uint16_t | SICK_SCAN_PROFILE_RANGE_AND_ECHO = 0x3DFF |
Request sector scan data w/ echo data. More... | |
static const uint8_t | SICK_SENSOR_MODE_ERROR = 0x04 |
The Sick LD is in error mode. More... | |
static const uint8_t | SICK_SENSOR_MODE_IDLE = 0x01 |
The Sick LD is powered but idle. More... | |
static const uint8_t | SICK_SENSOR_MODE_MEASURE = 0x03 |
The Sick LD prism is rotating, and the laser is on. More... | |
static const uint8_t | SICK_SENSOR_MODE_ROTATE = 0x02 |
The Sick LD prism is rotating, but laser is off. More... | |
static const uint8_t | SICK_SENSOR_MODE_UNKNOWN = 0xFF |
The Sick LD is in an unknown state. More... | |
static const uint8_t | SICK_SIGNAL_LED_GREEN = 0x04 |
Mask for green LED. More... | |
static const uint8_t | SICK_SIGNAL_LED_RED = 0x08 |
Mask for red LED. More... | |
static const uint8_t | SICK_SIGNAL_LED_YELLOW_A = 0x01 |
Mask for first yellow LED. More... | |
static const uint8_t | SICK_SIGNAL_LED_YELLOW_B = 0x02 |
Mask for second yellow LED. More... | |
static const uint8_t | SICK_SIGNAL_SWITCH_0 = 0x10 |
Mask for signal switch 0. More... | |
static const uint8_t | SICK_SIGNAL_SWITCH_1 = 0x20 |
Mask for signal switch 1. More... | |
static const uint8_t | SICK_SIGNAL_SWITCH_2 = 0x40 |
Mask for signal switch 2. More... | |
static const uint8_t | SICK_SIGNAL_SWITCH_3 = 0x80 |
Mask for signal switch 3. More... | |
static const uint8_t | SICK_STAT_SERV_CODE = 0x01 |
Status service code. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID = 0x01 |
Request the Sick LD ID. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_APP_NAME = 0x21 |
Request the application name. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_APP_PART_NUM = 0x20 |
Request the application part number. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_APP_VERSION = 0x22 |
Request the application version. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_FIRMWARE_NAME = 0x11 |
Request the firmware's name. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM = 0x10 |
Requess the firmware's part number. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION = 0x12 |
Request the firmware's version. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM = 0x04 |
Request the edm??? serial number. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_SENSOR_NAME = 0x01 |
Request the sensor's name. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM = 0x00 |
Request the sensor's part number. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM = 0x03 |
Request the sensor's serial number. More... | |
static const uint8_t | SICK_STAT_SERV_GET_ID_SENSOR_VERSION = 0x02 |
Request the sensor's version. More... | |
static const uint8_t | SICK_STAT_SERV_GET_SIGNAL = 0x04 |
Reads the value of the switch and LED port. More... | |
static const uint8_t | SICK_STAT_SERV_GET_STATUS = 0x02 |
Request status information. More... | |
static const uint8_t | SICK_STAT_SERV_LD_REGISTER_APPLICATION = 0x06 |
Registers the ID data for the application firmware. More... | |
static const uint8_t | SICK_STAT_SERV_SET_SIGNAL = 0x05 |
Sets the switches and LEDs. More... | |
static const uint8_t | SICK_WORK_SERV_CODE = 0x04 |
Working service code. More... | |
static const uint8_t | SICK_WORK_SERV_RESET = 0x01 |
Sick LD enters a reset sequence. More... | |
static const uint8_t | SICK_WORK_SERV_RESET_HALT_APP = 0x02 |
Sick LD does a minimal reset (Application is halted and device enters IDLE state) More... | |
static const uint8_t | SICK_WORK_SERV_RESET_INIT_CPU = 0x00 |
Sick LD does a complete reset (Reinitializes the CPU) More... | |
static const uint8_t | SICK_WORK_SERV_RESET_KEEP_CPU = 0x01 |
Sick LD does a partial reset (CPU is not reinitialized) More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_IDLE = 0x02 |
Sick LD enters IDLE mode (motor stops and laser is turned off) More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_MEASURE = 0x04 |
Sick LD enters MEASURE mode (laser starts with next revolution) More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE = 0x01 |
Sick LD reports config yields a max laser pulse frequency that is too high. More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE = 0x02 |
Sick LD reports config yields a max mean pulse frequency that is too high. More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER = 0x03 |
Sick LD reports sector borders are not configured correctly. More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT = 0x04 |
Sick LD reports sector borders are not a multiple of the step angle. More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_MEASURE_RET_OK = 0x00 |
Sick LD is ready to stream/obtain scan profiles. More... | |
static const uint8_t | SICK_WORK_SERV_TRANS_ROTATE = 0x03 |
Sick LD enters ROTATE mode (motor starts and rotates with a specified speed in Hz, laser is off) More... | |
Private Member Functions | |
uint16_t | _angleToTicks (const double angle) const |
Converts encoder ticks to equivalent angle representation. More... | |
void | _cancelSickScanProfiles () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Kills the current data stream. More... | |
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's manual) More... | |
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's manual) More... | |
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. More... | |
void | _flushTCPRecvBuffer () throw ( SickIOException, SickThreadException ) |
Flushes TCP receive buffer contents. More... | |
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. More... | |
void | _getApplicationSoftwareName () throw ( SickTimeoutException, SickIOException ) |
Get application software name. More... | |
void | _getApplicationSoftwarePartNumber () throw ( SickTimeoutException, SickIOException ) |
Get application software part number. More... | |
void | _getApplicationSoftwareVersion () throw ( SickTimeoutException, SickIOException ) |
Get application software part number. More... | |
void | _getFirmwareName () throw ( SickTimeoutException, SickIOException ) |
Get firmware name. More... | |
void | _getFirmwarePartNumber () throw ( SickTimeoutException, SickIOException ) |
Get firmware part number. More... | |
void | _getFirmwareVersion () throw ( SickTimeoutException, SickIOException ) |
Get firmware version number. More... | |
void | _getIdentificationString (const uint8_t id_request_code, std::string &id_return_string) throw ( SickTimeoutException, SickIOException ) |
Query the Sick LD for a particular ID string. More... | |
void | _getSensorEDMSerialNumber () throw ( SickTimeoutException, SickIOException ) |
Get sensor EDM serial number. More... | |
void | _getSensorName () throw ( SickTimeoutException, SickIOException ) |
Get the Sick LD's assigned sensor name. More... | |
void | _getSensorPartNumber () throw ( SickTimeoutException, SickIOException ) |
Get the Sick LD's part number. More... | |
void | _getSensorSerialNumber () throw ( SickTimeoutException, SickIOException ) |
Get the Sick LD's serial number. More... | |
void | _getSensorVersion () throw ( SickTimeoutException, SickIOException ) |
Get the Sick LD's sensor version. More... | |
void | _getSickEthernetConfig () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Get the Sick LD's Ethernet configuration. More... | |
void | _getSickGlobalConfig () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Get the global configuration of the Sick LD. More... | |
void | _getSickIdentity () throw ( SickTimeoutException, SickIOException ) |
Get the parameters that define the Sick LD's identity. More... | |
void | _getSickScanProfiles (const uint16_t profile_format, const uint16_t num_profiles=DEFAULT_SICK_NUM_SCAN_PROFILES) throw ( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) |
Request n scan profiles from the Sick LD unit. More... | |
void | _getSickSectorConfig () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Query the Sick for its current sector configuration. More... | |
void | _getSickSectorFunction (const uint8_t sector_num, uint8_t §or_function, double §or_stop_angle) throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Acquires the function of the given sector. More... | |
void | _getSickStatus () throw ( SickTimeoutException, SickIOException ) |
Get the status of the Sick LD. More... | |
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. More... | |
void | _printInitFooter () const |
Prints the initialization footer. More... | |
void | _printSectorProfileData (const sick_ld_sector_data_t §or_data) const |
Print data corresponding to the referenced sector data structure. More... | |
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. More... | |
void | _sendMessageAndGetReply (const SickLDMessage &send_message, SickLDMessage &recv_message, const unsigned int timeout_value=DEFAULT_SICK_MESSAGE_TIMEOUT) throw ( SickIOException, SickTimeoutException ) |
Send a message to the Sick LD and get its reply. More... | |
void | _setSickFilter (const uint8_t suppress_code) throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Enables/disables nearfield suppression on the Sick LD. More... | |
void | _setSickGlobalConfig (const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step) throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Sets the Sick LD's global parameters (sensor id, motor speed, and angular step) in flash. More... | |
void | _setSickGlobalParamsAndScanAreas (const unsigned int sick_motor_speed, const double sick_step_angle, const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors) throw ( SickTimeoutException, SickIOException, SickConfigException, SickErrorException ) |
Attempts to set the "permanent" (by writing flash) operating values for the device. More... | |
void | _setSickSectorConfig (const unsigned int *const sector_functions, const double *const sector_stop_angles, const unsigned int num_sectors, const bool write_to_flash=false) throw ( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) |
Sets the sector configuration for the device. More... | |
void | _setSickSectorFunction (const uint8_t sector_number, const uint8_t sector_function, const double sector_angle_stop, const bool write_to_flash=false) throw ( SickErrorException, SickTimeoutException, SickIOException, SickConfigException ) |
Sets the function for a particular scan sector. More... | |
void | _setSickSensorMode (const uint8_t new_sick_sensor_mode) throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Sets the Sick LD to the requested sensor mode. More... | |
void | _setSickSensorModeToIdle () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Sets the Sick LD sensor mode to IDLE. More... | |
void | _setSickSensorModeToMeasure () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Sets the Sick LD sensor mode to ROTATE. More... | |
void | _setSickSensorModeToRotate () throw ( SickErrorException, SickTimeoutException, SickIOException ) |
Sets the Sick LD sensor mode to ROTATE. More... | |
void | _setSickSignals (const uint8_t sick_signal_flags=DEFAULT_SICK_SIGNAL_SET) throw ( SickIOException, SickTimeoutException, SickErrorException ) |
Sets the Sick LEDs and switching outputs. More... | |
void | _setSickTemporaryScanAreas (const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors) throw ( SickTimeoutException, SickIOException, SickConfigException ) |
Attempts to set the "temporary" (until a device reset) scan area config for the device. More... | |
void | _setupConnection () throw ( SickIOException, SickTimeoutException ) |
Establish a TCP connection to the unit. More... | |
std::string | _sickMotorModeToString (const uint8_t sick_motor_mode) const |
Converts the Sick LD numerical motor mode to a representative string. More... | |
std::string | _sickProfileFormatToString (const uint16_t profile_format) const |
Converts the Sick LD numerical motor mode to a representative string. More... | |
std::string | _sickResetLevelToString (const uint16_t reset_level) const |
Converts the Sick LD numerical reset level to a representative string. More... | |
std::string | _sickSectorFunctionToString (const uint16_t sick_sector_function) const |
Converts the Sick LD numerical sector config to a representative string. More... | |
std::string | _sickSensorModeToString (const uint8_t sick_sensor_mode) const |
Converts the Sick LD numerical sensor mode to a representative string. More... | |
uint8_t | _sickSensorModeToWorkServiceSubcode (const uint8_t sick_sensor_mode) const |
Map Sick LD sensor modes to their equivalent service subcode representations. More... | |
std::string | _sickTransMeasureReturnToString (const uint8_t return_value) const |
Converts return value from TRANS_MEASURE to a representative string. More... | |
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. More... | |
bool | _supportedScanProfileFormat (const uint16_t profile_format) const |
Check that the given profile format is supported by the current driver version. More... | |
void | _syncDriverWithSick () throw ( SickIOException, SickTimeoutException, SickErrorException ) |
Synchronizes buffer driver parameters with those of the Sick LD. More... | |
void | _teardownConnection () throw ( SickIOException ) |
Teardown TCP connection to Sick LD. More... | |
double | _ticksToAngle (const uint16_t ticks) const |
Converts encoder ticks to equivalent angle representation. More... | |
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. More... | |
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 sector config) More... | |
bool | _validPulseFrequency (const unsigned int sick_motor_speed, const double sick_step_angle, const double *const active_sector_start_angles, const double *const active_sector_stop_angles, const unsigned int num_active_sectors) const |
Checks whether the given configuration yields a valid mean and max pulse frequency (uses given sector config) More... | |
bool | _validSickMotorSpeed (const unsigned int sick_motor_speed) const |
Checks whether the given sick motor speed is valid for the device. More... | |
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. More... | |
bool | _validSickSensorID (const unsigned int sick_sensor_id) const |
Checks whether the given senor id is valid for the device. More... | |
Private Attributes | |
sick_ld_config_ethernet_t | _sick_ethernet_config |
sick_ld_config_global_t | _sick_global_config |
sick_ld_identity_t | _sick_identity |
struct sockaddr_in | _sick_inet_address_info |
std::string | _sick_ip_address |
uint8_t | _sick_motor_mode |
sick_ld_config_sector_t | _sick_sector_config |
uint8_t | _sick_sensor_mode |
bool | _sick_streaming_range_and_echo_data |
bool | _sick_streaming_range_data |
uint16_t | _sick_tcp_port |
unsigned int | _socket |
Additional Inherited Members | |
Protected Member Functions inherited from SickToolbox::SickLIDAR< SickLDBufferMonitor, SickLDMessage > | |
double | _computeElapsedTime (const struct timeval &beg_time, const struct timeval &end_time) const |
bool | _monitorRunning () const |
void | _recvMessage (SickLDMessage &sick_message, const unsigned int timeout_value) const throw ( SickTimeoutException ) |
Attempt to acquire the latest available message from the device. More... | |
void | _recvMessage (SickLDMessage &sick_message, const uint8_t *const byte_sequence, const unsigned int byte_sequence_length, const unsigned int timeout_value) const throw ( SickTimeoutException ) |
Attempt to acquire a message having a payload beginning w/ the given byte sequence. More... | |
void | _sendMessage (const SickLDMessage &sick_message, const unsigned int byte_interval) const throw ( SickIOException ) |
Sends a message to the Sick device. More... | |
virtual void | _sendMessageAndGetReply (const SickLDMessage &send_message, SickLDMessage &recv_message, const uint8_t *const byte_sequence, const unsigned int byte_sequence_length, const unsigned int byte_interval, const unsigned int timeout_value, const unsigned int num_tries) throw ( SickTimeoutException, SickIOException) |
void | _setBlockingIO () const throw ( SickIOException ) |
A simple method for setting blocking I/O. More... | |
void | _setNonBlockingIO () const throw ( SickIOException ) |
A simple method for setting non-blocking I/O. More... | |
void | _startListening () throw ( SickThreadException ) |
Activates the buffer monitor for the driver. More... | |
void | _stopListening () throw ( SickThreadException ) |
Activates the buffer monitor for the driver. More... | |
Protected Attributes inherited from SickToolbox::SickLIDAR< SickLDBufferMonitor, SickLDMessage > | |
SickLDBufferMonitor * | _sick_buffer_monitor |
int | _sick_fd |
bool | _sick_initialized |
bool | _sick_monitor_running |
Provides a simple driver interface for working with the Sick LD-OEM/Sick LD-LRS long-range models via Ethernet.
Adopt c-style convention.
Adopt c-style convention.
Adopt c-style convention.
Adopt c-style convention.
Adopt c-style convention.
Adopt c-style convention.
SickToolbox::SickLD::SickLD | ( | const std::string | sick_ip_address = DEFAULT_SICK_IP_ADDRESS , |
const uint16_t | sick_tcp_port = DEFAULT_SICK_TCP_PORT |
||
) |
SickToolbox::SickLD::~SickLD | ( | ) |
|
private |
Converts encoder ticks to equivalent angle representation.
Converts angle to an equivalent representation in odometer ticks
ticks | The tick value to be converted |
NOTE: This function assumes that the angle value it receives is a multiple of 1/16 degree (which is the resolution of the encoder)
|
private |
|
private |
Compute the mean pulse frequency (see page 22 of the operator's manual)
Computes the total pulse frequency for the given config
active_scan_area | The total scan area that can be covered by the Sick |
curr_motor_speed | The current motor speed (in Hz) |
curr_angular_resolution | The current angular resolution of the Sick (in deg) |
|
private |
Compute the mean pulse frequency (see page 22 of the operator's manual)
Computes the mean pulse frequency for the given config
active_scan_area | The total area where the Sick is actively scanning (in deg) (i.e. total area where the laser isn't blanked) |
curr_motor_speed | The current motor speed (in Hz) |
curr_angular_resolution | The current angular resolution of the Sick (in deg) |
|
private |
Computes the active scan area for the Sick given the current sector configuration.
Returns the scanning area for the device given the current sector configuration
sick_angle_step | The angular resolution of the Sick LD |
active_sector_start_angles | The start angles for the active scan sectors |
active_sector_stop_angles | The stop angles for the active scan sectors |
num_active_sectors | The number of active sectors |
NOTE: The Sick LD computes scan area by subtracting the end of the previous sector from the end of the current sector. As such, we have to add a single step angle to our computation in order to get the scan area that is used by the Sick LD. Unfortunately, this is how the device does its computation (as opposed to using the angle at which the first scan in the given sector is taken) so this is how we do it.
|
private |
|
private |
Generates a device-ready sector set given only an active sector spec.
Generates a device-ready sector set given only an active sector spec.
*active_sector_start_angles | Start angles for the active (measuring) sectors |
*active_sector_stop_angles | Stop angles for the active sectors |
num_active_sectors | The number of active sectors given |
sick_angle_step | The step angle (angular resolution) to be used in generating the config |
*sector_functions | An output buffer to hold the function assigned to each sector |
*sector_stop_angles | An output buffer to hold the stop angles associated w/ the generated sector set |
&num_sectors | The number of sectors in the final device-ready configuration |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
Request n scan profiles from the Sick LD unit.
Requests n range measurement profiles from the Sick LD
profile_format | The format for the requested scan profiles |
num_profiles | The number of profiles to request from Sick LD. (Default: 0) (NOTE: When num_profiles = 0, the Sick LD continuously streams profile data) |
|
private |
Query the Sick for its current sector configuration.
Acquires the configuration (function and stop angle) for each sector
NOTE: Here we only buffer the sector stop angle as this is the only value returned by the GET_FUNCTION command. Using the stop angle does not give enough information to extract the start angle of sectors that aren't "normal measurement".
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
Prints data fields of the given scan profile.
Prints the data corresponding to the given scan profile (for debugging purposes)
profile_data | The scan profile to be printed. |
print_sector_data | Indicates whether to print the sector data fields associated with the given profile. |
|
private |
Send a message to the Sick LD and get its reply.
Send a message, get the reply from the Sick LD and check it
&send_message | A reference to the well-formed message object that is to be sent. |
&recv_message | The destination message object for the received reply. |
timeout_value | The maximum time to allow for a response in seconds |
NOTE: This method also verifies the correct reply to the given send_message object is received. So, if it fails, it may be due to an unexpected reply.
|
private |
|
private |
Sets the Sick LD's global parameters (sensor id, motor speed, and angular step) in flash.
Sets the Sick LD's global configuration (in flash)
sick_sensor_id | The sensor id to be assigned to the unit. |
sick_motor_speed | The speed of the motor in Hz (must be in [5,20]) |
sick_angular_step | The difference between two laser pulses in 1/16 degrees (e.g. sick_angular_step = 4 => 0.25 deg step). Also, this value must evenly divide into 5760 and be greater than 1 (see telegram listing page 21). |
ALERT: This method writes the parameters to the Sick LD's flash, so there is no need to use it except when configuring the device.
ALERT: This method DOES NOT DO ERROR CHECKING on the given parameter values. It is the responsibility of the caller function to do error checking beforehand.
|
private |
Attempts to set the "permanent" (by writing flash) operating values for the device.
Allows setting the global parameters and scan area definition (in flash)
sick_motor_speed | Desired motor speed for the device (hz) |
sick_angle_step | Desired scan angular resolution (deg) |
active_sector_start_angles | Angles marking the beginning of each desired active sector/area |
active_sector_stop_angles | Angles marking the end of each desired active sector/area |
num_active_sectors | The number of active sectors |
|
private |
Sets the sector configuration for the device.
Sets the sick sector configuration
sector_functions | Angles marking the beginning of each desired active sector/area |
sector_stop_angles | Angles marking the end of each desired active sector/area |
num_sectors | The total number of sectors in the configuration |
set_flash_flag | Indicates whether to mark the sectors for writing to flash w/ the next SET_CONFIG (global) |
|
private |
Sets the function for a particular scan sector.
Set the function for a particular scan secto
sector_number | The number of the sector (should be in [0,7]) |
sector_function | The function of the sector (e.g. no measurement, reserved, normal measurement, ...) |
sector_stop_angle | The last angle of the sector (in odometer ticks) |
write_to_flash | Indicates whether the sector configuration should be written to flash |
|
private |
|
private |
|
private |
|
private |
|
private |
Sets the Sick LEDs and switching outputs.
Sets the signals for the device
sick_signal_flags | Flags indicating which LEDs and SWITCHES to activate |
NOTE: This method does not preserve the previous state of the Sick's signals. In other words, only the signals flagged in sick_signal_flags will be set - all others will be off!
|
private |
Attempts to set the "temporary" (until a device reset) scan area config for the device.
Allows setting a temporary (until a device reset) sector configuration on the device
active_sector_start_angles | Angles marking the beginning of each desired active sector/area |
active_sector_stop_angles | Angles marking the end of each desired active sector/area |
num_active_sectors | The number of active sectors |
|
privatevirtual |
Establish a TCP connection to the unit.
Setup the connection parameters and establish TCP connection!
Implements SickToolbox::SickLIDAR< SickLDBufferMonitor, SickLDMessage >.
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
Map Sick LD sensor modes to their equivalent service subcode representations.
Returns the corresponding work service subcode required to transition the Sick LD to the given sensor mode.
sick_sensor_mode | The sensor mode to be converted |
|
private |
|
private |
Sort the scan areas based on the given angles to place them in device "scan" order.
Reorders given sector angle sets
sector_start_angles | Array of angles (deg) defining the starting position of each active sector |
sector_stop_angles | Array of angles (deg) defining the stopping position of each active sector |
num_active_sectors | Number of active sectors |
|
private |
Check that the given profile format is supported by the current driver version.
Indicates whether the supplied profile format is currently supported by the driver
profile_format | The requested profile format. |
|
private |
|
privatevirtual |
Teardown TCP connection to Sick LD.
Teardown the connection to the Sick LD
Implements SickToolbox::SickLIDAR< SickLDBufferMonitor, SickLDMessage >.
|
private |
|
private |
Determines wheter a given set of sector bounds are valid.
Checks the given sector arguments for overlapping regions yielding an invalid configuration
sector_start_angles | Array of angles (deg) defining the starting position of each active sector |
sector_stop_angles | Array of angles (deg) defining the stopping position of each active sector |
num_active_sectors | Number of active sectors |
|
private |
Checks whether the given configuration yields a valid mean and max pulse frequency (uses current sector config)
Indicates whether the given configuration yields a valid max and mean pulse frequency
sick_motor_speed | Desired sick motor speed (Hz) |
sick_angle_step | Desired scan angular resolution (deg) |
|
private |
Checks whether the given configuration yields a valid mean and max pulse frequency (uses given sector config)
Indicates whether the given configuration yields a valid max and mean pulse frequency
sick_motor_speed | Desired sick motor speed (Hz) |
sick_angle_step | Desired scan angular resolution (deg) |
active_sector_start_angles | Angles marking the beginning of each desired active sector/area |
active_sector_stop_angles | Angles marking the end of each desired active sector/area |
num_active_sectors | The number of active sectors/scan areas are given |
|
private |
|
private |
Checks whether the given scan resolution is valid.
Indicates whether a given motor speed is valid for the device
sick_scan_resolution | Scan resolution of the device |
sector_start_angles | An array of the sector start angles |
sector_stop_angles | An array of the sector stop angles |
|
private |
void SickToolbox::SickLD::DisableNearfieldSuppression | ( | ) | ||
throw | ( | SickErrorException, | ||
SickTimeoutException, | ||||
SickIOException | ||||
) |
void SickToolbox::SickLD::EnableNearfieldSuppression | ( | ) | ||
throw | ( | SickErrorException, | ||
SickTimeoutException, | ||||
SickIOException | ||||
) |
std::string SickToolbox::SickLD::GetSickAppSoftwareName | ( | ) | const |
std::string SickToolbox::SickLD::GetSickAppSoftwarePartNumber | ( | ) | const |
std::string SickToolbox::SickLD::GetSickAppSoftwareVersionNumber | ( | ) | const |
std::string SickToolbox::SickLD::GetSickEDMSerialNumber | ( | ) | const |
std::string SickToolbox::SickLD::GetSickEthernetConfigAsString | ( | ) | const |
std::string SickToolbox::SickLD::GetSickFirmwareName | ( | ) | const |
std::string SickToolbox::SickLD::GetSickFirmwarePartNumber | ( | ) | const |
std::string SickToolbox::SickLD::GetSickFirmwareVersion | ( | ) | const |
std::string SickToolbox::SickLD::GetSickGatewayIPAddress | ( | ) | const |
std::string SickToolbox::SickLD::GetSickGlobalConfigAsString | ( | ) | const |
std::string SickToolbox::SickLD::GetSickIdentityAsString | ( | ) | const |
std::string SickToolbox::SickLD::GetSickIPAddress | ( | ) | const |
void SickToolbox::SickLD::GetSickMeasurements | ( | double *const | range_measurements, |
unsigned int *const | echo_measurements = NULL , |
||
unsigned int *const | num_measurements = NULL , |
||
unsigned int *const | sector_ids = NULL , |
||
unsigned int *const | sector_data_offsets = NULL , |
||
double *const | sector_step_angles = NULL , |
||
double *const | sector_start_angles = NULL , |
||
double *const | sector_stop_angles = NULL , |
||
unsigned int *const | sector_start_timestamps = NULL , |
||
unsigned int *const | sector_stop_timestamps = NULL |
||
) | |||
throw | ( | SickErrorException, | |
SickIOException, | |||
SickTimeoutException, | |||
SickConfigException | |||
) |
Acquires measurements and corresponding sector data from the Sick LD.
Acquires measurements and related data for all active sectors
*range_measurements | A single array to hold ALL RANGE MEASUREMENTS from the current scan for all active sectors. Range values from each sector are stored block sequentially in this buffer. The respective index into the array marking the first data value returned by active sector i can be found by getting: range_measurement[sector_data_offsets[i]]. |
*echo_measurements | A single array to hold ALL ECHO MEASUREMENTS from the current scan for all active sectors. It is indexed the same as range_measurements. This argument is optional and if it is not provided the driver will request a RANGE ONLY data stream as opposed to a RANGE+ECHO thereby reducing consumed bandwidth (Default: NULL). |
*num_measurements | An array where the ith element denotes the number of range/echo measurements obtained from active sector i (Default: NULL). |
*sector_ids | An array where the ith element corresponds to the actual sector number/id of the ith active sector. (Default: NULL) |
*sector_data_offsets | The index offsets mapping the ith active sector to the position in range_measurements where its first measured value can be found (Default: NULL). |
*sector_step_angles | An array where the ith element corresponds to the angle step for the ith active sector. (Default: NULL) |
*sector_start_angles | An array where the ith element corresponds to the starting scan angle of the ith active sector. |
*sector_stop_angles | An array where the ith element corresponds to the stop scan angle of the ith active sector. |
*sector_start_timestamps | An array where the ith element denotes the time at which the first scan was taken for the ith active sector. |
*sector_stop_timestamps | An array where the ith element denotes the time at which the last scan was taken for the ith active sector. |
ALERT: The user is responsible for ensuring that enough space is allocated for the return buffers to avoid overflow. See the example code for an easy way to do this.
unsigned int SickToolbox::SickLD::GetSickMotorSpeed | ( | ) | const |
std::string SickToolbox::SickLD::GetSickName | ( | ) | const |
unsigned int SickToolbox::SickLD::GetSickNumActiveSectors | ( | ) | const |
std::string SickToolbox::SickLD::GetSickPartNumber | ( | ) | const |
double SickToolbox::SickLD::GetSickScanArea | ( | ) | const |
double SickToolbox::SickLD::GetSickScanResolution | ( | ) | const |
std::string SickToolbox::SickLD::GetSickSectorConfigAsString | ( | ) | const |
unsigned int SickToolbox::SickLD::GetSickSensorID | ( | ) | const |
std::string SickToolbox::SickLD::GetSickSerialNumber | ( | ) | const |
void SickToolbox::SickLD::GetSickSignals | ( | uint8_t & | sick_signal_flags | ) | |
throw | ( | SickIOException, | |||
SickTimeoutException | |||||
) |
void SickToolbox::SickLD::GetSickStatus | ( | unsigned int & | sick_sensor_mode, |
unsigned int & | sick_motor_mode | ||
) | |||
throw | ( | SickIOException, | |
SickTimeoutException | |||
) |
std::string SickToolbox::SickLD::GetSickStatusAsString | ( | ) | const |
std::string SickToolbox::SickLD::GetSickSubnetMask | ( | ) | const |
void SickToolbox::SickLD::GetSickTime | ( | uint16_t & | sick_time | ) | |
throw | ( | SickIOException, | |||
SickTimeoutException, | |||||
SickErrorException | |||||
) |
std::string SickToolbox::SickLD::GetSickVersion | ( | ) | const |
void SickToolbox::SickLD::Initialize | ( | ) | ||
throw | ( | SickIOException, | ||
SickThreadException, | ||||
SickTimeoutException, | ||||
SickErrorException | ||||
) |
void SickToolbox::SickLD::PrintSickEthernetConfig | ( | ) | const |
void SickToolbox::SickLD::PrintSickGlobalConfig | ( | ) | const |
void SickToolbox::SickLD::PrintSickIdentity | ( | ) | const |
void SickToolbox::SickLD::PrintSickSectorConfig | ( | ) | const |
void SickToolbox::SickLD::PrintSickStatus | ( | ) | const |
void SickToolbox::SickLD::ResetSick | ( | const unsigned int | reset_level = SICK_WORK_SERV_RESET_INIT_CPU | ) | |
throw | ( | SickErrorException, | |||
SickTimeoutException, | |||||
SickIOException, | |||||
SickConfigException | |||||
) |
void SickToolbox::SickLD::SetSickGlobalParamsAndScanAreas | ( | const unsigned int | sick_motor_speed, |
const double | sick_angle_step, | ||
const double *const | active_sector_start_angles, | ||
const double *const | active_sector_stop_angles, | ||
const unsigned int | num_active_sectors | ||
) | |||
throw | ( | SickTimeoutException, | |
SickIOException, | |||
SickConfigException, | |||
SickErrorException | |||
) |
Attempts to set the scan resolution and active sectors/scan areas for the device (in flash)
Attempts to set the global params and the active scan sectors for the device (in flash)
sick_motor_speed | Desired sick motor speed (Hz) |
sick_angle_step | Desired scan angular resolution (deg) |
active_sector_start_angles | Angles marking the beginning of each desired active sector/area |
active_sector_stop_angles | Angles marking the end of each desired active sector/area |
num_active_sectors | The number of active sectors/scan areas are given |
void SickToolbox::SickLD::SetSickMotorSpeed | ( | const unsigned int | sick_motor_speed | ) | |
throw | ( | SickErrorException, | |||
SickTimeoutException, | |||||
SickIOException | |||||
) |
void SickToolbox::SickLD::SetSickScanAreas | ( | const double *const | active_sector_start_angles, |
const double *const | active_sector_stop_angles, | ||
const unsigned int | num_active_sectors | ||
) | |||
throw | ( | SickTimeoutException, | |
SickIOException, | |||
SickConfigException, | |||
SickErrorException | |||
) |
Attempts to set the active scan areas for the device (in flash)
Attempts to set the active scan sectors for the device (in flash)
active_sector_start_angles | Angles marking the beginning of each desired active sector/area |
active_sector_stop_angles | Angles marking the end of each desired active sector/area |
num_active_sectors | The number of active sectors/scan areas are given |
void SickToolbox::SickLD::SetSickScanResolution | ( | const double | sick_angle_step | ) | |
throw | ( | SickTimeoutException, | |||
SickIOException, | |||||
SickConfigException | |||||
) |
Attempts to set a new scan resolution for the device (in flash) while retaining the previously defined active scan areas.
Attempts to set a new scan resolution for the device (in flash)
sick_angle_step | The desired scan resolution (deg) |
NOTE: Since the Sick determines the active sector bounds based upon the current scan resolution and sector stop angles, it is possible that when the scan resolution is changed so too are the active sector bounds. As such, we also write the correct sector config to flash as opposed to letting the Sick LD just figure it out itself. Doing so ensures the previous active sector definitions remain intact.
void SickToolbox::SickLD::SetSickSensorID | ( | const unsigned int | sick_sensor_id | ) | |
throw | ( | SickErrorException, | |||
SickTimeoutException, | |||||
SickIOException | |||||
) |
void SickToolbox::SickLD::SetSickSignals | ( | const uint8_t | sick_signal_flags = DEFAULT_SICK_SIGNAL_SET | ) | |
throw | ( | SickIOException, | |||
SickTimeoutException, | |||||
SickErrorException | |||||
) |
Sets the Sick LD signal LED's and switching outputs.
Sets the signal LEDs and switches
sick_signal_flags | Indicates the LEDs and switches to set/unset. |
NOTE: This method does not preserve the previous state of the Sick's signals. In other words, only the signals flagged in sick_signal_flags will be set - all others will be off!
void SickToolbox::SickLD::SetSickTempScanAreas | ( | const double * | active_sector_start_angles, |
const double *const | active_sector_stop_angles, | ||
const unsigned int | num_active_sectors | ||
) | |||
throw | ( | SickTimeoutException, | |
SickIOException, | |||
SickConfigException | |||
) |
Set the temporary scan areas for the Sick LD.
Sets the temporal scan configuration (until power is cycled)
*active_sector_start_angles | Angles marking the beginning of each desired active sector/area |
*active_sector_stop_angles | Angles marking the end of each desired active sector/area |
num_active_sectors | The number of active sectors |
NOTE: The active scan areas set at this point are only maintained until a device reset occurs. In other words, they are not written to flash.
void SickToolbox::SickLD::SetSickTimeAbsolute | ( | const uint16_t | absolute_clock_time, |
uint16_t & | new_sick_clock_time | ||
) | |||
throw | ( | SickErrorException, | |
SickTimeoutException, | |||
SickIOException, | |||
SickConfigException | |||
) |
Set the absolute time of the Sick LD unit.
Sets the internal clock of the Sick LD unit
absolute_clock_time | The absolute clock time in milliseconds |
new_sick_clock_time | The clock time in milliseconds returned from the device |
ALERT: If the Sick LD is in MEASUREMENT mode, this method will first set the device to ROTATE mode after killing any active data streams (as per the protocol).
void SickToolbox::SickLD::SetSickTimeRelative | ( | const int16_t | delta_time, |
uint16_t & | new_sick_clock_time | ||
) | |||
throw | ( | SickErrorException, | |
SickTimeoutException, | |||
SickIOException, | |||
SickConfigException | |||
) |
Set the relative time of the Sick LD unit.
Sets the internal clock of the Sick LD using the relative given time value
delta_time | The relative clock time in milliseconds. |
&new_sick_clock_time | The new time as reported by the Sick LD. |
ALERT: If the Sick LD is in MEASUREMENT mode, this method will first set the device to ROTATE mode after killing any active data streams (as per the protocol).
void SickToolbox::SickLD::Uninitialize | ( | ) | ||
throw | ( | SickIOException, | ||
SickTimeoutException, | ||||
SickErrorException, | ||||
SickThreadException | ||||
) |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |
|
static |