Public Types | |
enum | { MAX_SCANNODE_CACHE_COUNT = 8192 } |
enum | { A2A3_LIDAR_MINUM_MAJOR_ID = 2, BUILTIN_MOTORCTL_MINUM_MAJOR_ID = 6 } |
enum | { TOF_C_SERIAL_MINUM_MAJOR_ID = 4, TOF_S_SERIAL_MINUM_MAJOR_ID = 6, TOF_T_SERIAL_MINUM_MAJOR_ID = 9, TOF_M_SERIAL_MINUM_MAJOR_ID = 12, NEWDESIGN_MINUM_MAJOR_ID = TOF_C_SERIAL_MINUM_MAJOR_ID } |
![]() | |
enum | { DEFAULT_TIMEOUT = 2000 } |
Public Member Functions | |
sl_result | ascendScanData (sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t count) |
sl_result | checkMotorCtrlSupport (MotorCtrlSupport &support, sl_u32 timeout=DEFAULT_TIMEOUT) |
sl_result | connect (IChannel *channel) |
void | disconnect () |
sl_result | getAllSupportedScanModes (std::vector< LidarScanMode > &outModes, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
Get all scan modes that supported by lidar. More... | |
sl_result | getDeviceInfo (sl_lidar_response_device_info_t &info, sl_u32 timeout=DEFAULT_TIMEOUT) |
sl_result | getDeviceMacAddr (sl_u8 *macAddrArray, sl_u32 timeoutInMs) |
sl_result | getFrequency (const LidarScanMode &scanMode, const sl_lidar_response_measurement_node_hq_t *nodes, size_t count, float &frequency) |
sl_result | getHealth (sl_lidar_response_device_health_t &health, sl_u32 timeout=DEFAULT_TIMEOUT) |
sl_result | getLidarIpConf (sl_lidar_ip_conf_t &conf, sl_u32 timeout) |
LIDARMajorType | getLIDARMajorType (const sl_lidar_response_device_info_t *devInfo) |
LIDARTechnologyType | getLIDARTechnologyType (const sl_lidar_response_device_info_t *devInfo) |
sl_result | getModelNameDescriptionString (std::string &out_description, bool fetchAliasName, const sl_lidar_response_device_info_t *devInfo, sl_u32 timeout) |
sl_result | getMotorInfo (LidarMotorInfo &motorInfo, sl_u32 timeoutInMs) |
sl_result | getScanDataWithIntervalHq (sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &count) |
sl_result | getTypicalScanMode (sl_u16 &outMode, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
Get typical scan mode of lidar. More... | |
sl_result | grabScanDataHq (sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &count, sl_u32 timeout=DEFAULT_TIMEOUT) |
sl_result | grabScanDataHqWithTimeStamp (sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &count, sl_u64 ×tamp_uS, sl_u32 timeout=DEFAULT_TIMEOUT) |
bool | isConnected () |
sl_result | negotiateSerialBaudRate (sl_u32 requiredBaudRate, sl_u32 *baudRateDetected) |
virtual void | onHQNodeDecoded (_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t *node) |
virtual void | onHQNodeScanResetReq () |
virtual void | onProtocolMessageDecoded (const internal::ProtocolMessage &msg) |
sl_result | reset (sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
sl_result | setLidarIpConf (const sl_lidar_ip_conf_t &conf, sl_u32 timeout) |
sl_result | setMotorSpeed (sl_u16 speed=DEFAULT_MOTOR_SPEED) |
SlamtecLidarDriver () | |
sl_result | startScan (bool force, bool useTypicalScan, sl_u32 options=0, LidarScanMode *outUsedScanMode=nullptr) |
sl_result | startScanExpress (bool force, sl_u16 scanMode, sl_u32 options=0, LidarScanMode *outUsedScanMode=nullptr, sl_u32 timeout=DEFAULT_TIMEOUT) |
sl_result | startScanNormal (bool force, sl_u32 timeout=DEFAULT_TIMEOUT) |
sl_result | startScanNormal_commonpath (bool force, bool ifSupportLidarConf, LidarScanMode &outUsedScanMode, sl_u32 timeout) |
sl_result | stop (sl_u32 timeout=DEFAULT_TIMEOUT) |
virtual | ~SlamtecLidarDriver () |
![]() | |
virtual | ~ILidarDriver () |
Protected Member Functions | |
bool | _checkNDMagicNumber (_u8 model) |
u_result | _detectLIDARNativeInterfaceType (LIDARInterfaceType &outputType, const rplidar_response_device_info_t &devInfo, sl_u32 timeout=DEFAULT_TIMEOUT) |
void | _disableDataGrabbing () |
u_result | _getLegacySampleDuration_uS (rplidar_response_sample_rate_t &rateInfo, _u32 timeout) |
_u32 | _getNativeBaudRate (const rplidar_response_device_info_t &devInfo) |
u_result | _sendCommandWithoutResponse (_u8 cmd, const void *payload=NULL, size_t payloadsize=0, bool noForceStop=false) |
u_result | _sendCommandWithResponse (_u8 cmd, _u8 responseType, internal::message_autoptr_t &ansPkt, _u32 timeout=DEFAULT_TIMEOUT, const void *payload=NULL, size_t payloadsize=0) |
bool | _updateTimingDesc (const rplidar_response_device_info_t &devInfo, float selectedSampleDuration) |
u_result | checkSupportConfigCommands (bool &outSupport, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | getDesiredSpeed (sl_lidar_response_desired_rot_speed_t &motorSpeed, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | getLidarConf (_u32 type, std::vector< _u8 > &outputBuf, const void *payload=NULL, size_t payloadSize=0, _u32 timeout=DEFAULT_TIMEOUT) |
u_result | getLidarSampleDuration (float &sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | getMaxDistance (float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | getScanModeAnsType (sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | getScanModeCount (sl_u16 &modeCount, sl_u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | getScanModeName (char *modeName, size_t stringSize, _u16 scanModeID, _u32 timeoutInMs=DEFAULT_TIMEOUT) |
u_result | setLidarConf (_u32 type, const void *payload, size_t payloadSize, _u32 timeout) |
sl_result | startMotor () |
Static Protected Member Functions | |
static std::string | GetModelNameStringByModelID (_u8 modelID) |
static LIDARMajorType | ParseLIDARMajorTypeByModelID (_u8 modelID) |
static LIDARTechnologyType | ParseLIDARTechnologyTypeByModelID (_u8 modelID) |
Private Attributes | |
sl_lidar_response_device_info_t | _cached_DevInfo |
rp::hal::Locker | _data_locker |
std::shared_ptr< internal::LIDARSampleDataUnpacker > | _dataunpacker |
bool | _isConnected |
MotorCtrlSupport | _isSupportingMotorCtrl |
internal::message_autoptr_t | _lastAnsPkt |
rp::hal::Locker | _op_locker |
std::shared_ptr< internal::RPLidarProtocolCodec > | _protocolHandler |
RawSampleNodeHolder< sl_lidar_response_measurement_node_hq_t > | _rawSampleNodeHolder |
rp::hal::Waiter< _u32 > | _response_waiter |
ScanDataHolder< sl_lidar_response_measurement_node_hq_t > | _scanHolder |
SlamtecLidarTimingDesc | _timing_desc |
std::shared_ptr< internal::AsyncTransceiver > | _transeiver |
_u32 | _waiting_packet_type |
Additional Inherited Members |
Definition at line 373 of file sl_lidar_driver.cpp.
anonymous enum |
Enumerator | |
---|---|
MAX_SCANNODE_CACHE_COUNT |
Definition at line 377 of file sl_lidar_driver.cpp.
anonymous enum |
Enumerator | |
---|---|
A2A3_LIDAR_MINUM_MAJOR_ID | |
BUILTIN_MOTORCTL_MINUM_MAJOR_ID |
Definition at line 381 of file sl_lidar_driver.cpp.
anonymous enum |
Enumerator | |
---|---|
TOF_C_SERIAL_MINUM_MAJOR_ID | |
TOF_S_SERIAL_MINUM_MAJOR_ID | |
TOF_T_SERIAL_MINUM_MAJOR_ID | |
TOF_M_SERIAL_MINUM_MAJOR_ID | |
NEWDESIGN_MINUM_MAJOR_ID |
Definition at line 387 of file sl_lidar_driver.cpp.
|
inline |
Definition at line 398 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Definition at line 416 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1467 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1475 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1459 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1556 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1516 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1600 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1612 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1538 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Ascending the scan data according to the angle value in the scan.
nodebuffer | Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData |
count | The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). Once the interface returns, this parameter will store the actual received data count. The interface will return SL_RESULT_OPERATION_FAIL when all the scan data is invalid. |
Implements sl::ILidarDriver.
Definition at line 957 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Check whether the device support motor control Note: this API will disable grab.
motorCtrlSupport | Return the result. |
timeout | The operation timeout value (in millisecond) for the serial port communication. |
Implements sl::ILidarDriver.
Definition at line 833 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1176 of file sl_lidar_driver.cpp.
Connect to LIDAR via channel
channel | The communication channel Note: you should manage the lifecycle of the channel object, make sure it is alive during lidar driver's lifecycle |
Implements sl::ILidarDriver.
Definition at line 473 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Disconnect from the LIDAR
Implements sl::ILidarDriver.
Definition at line 495 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get all scan modes that supported by lidar.
Implements sl::ILidarDriver.
Definition at line 518 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1163 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
info | The device information returned from the RPLIDAR |
timeout | The operation timeout value (in millisecond) for the serial port communication |
Implements sl::ILidarDriver.
Definition at line 808 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get LPX series lidar's MAC address
macAddrArray | The device MAC information returned from the LPX series lidar Notice: the macAddrArray must point to a valid buffer with at least 6 bytes length Otherwise, buffer overwrite will occur |
Implements sl::ILidarDriver.
Definition at line 937 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Calculate LIDAR's current scanning frequency from the given scan data Please refer to the application note doc for details Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
scanMode | Lidar's current scan mode |
nodes | Current scan's measurements |
count | The number of sample nodes inside the given buffer |
Implements sl::ILidarDriver.
Definition at line 880 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Retrieve the health status of the RPLIDAR The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
health | The health status info returned from the RPLIDAR |
timeout | The operation timeout value (in millisecond) for the serial port communication |
Implements sl::ILidarDriver.
Definition at line 913 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1261 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get LPX and S2E series lidar's static IP address
conf | Network parameter that LPX series lidar owned |
timeout | The operation timeout value (in millisecond) for the ethernet udp communication |
Implements sl::ILidarDriver.
Definition at line 896 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get the Major Type (Series Info) of the LIDAR
devInfo | The device info used to deduct the result If NULL is specified, a driver cached version of the connected LIDAR will be used |
Implements sl::ILidarDriver.
Definition at line 433 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1306 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get the technology of the LIDAR's measurement system
devInfo | The device info used to deduct the result If NULL is specified, a driver cached version of the connected LIDAR will be used |
Implements sl::ILidarDriver.
Definition at line 423 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1325 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get the Model Name of the LIDAR The result will be somthing like: "A1M8" or "S1M1" or "A3M1-R1"
out_description | The output string that contains the generated model name |
fetchAliasName | If set to true, a communication will be taken to ask if there is any Alias name availabe |
devInfo | The device info used to deduct the result If NULL is specified, a driver cached version of the connected LIDAR will be used |
timeout | The timeout value used by potential data communication |
Implements sl::ILidarDriver.
Definition at line 444 of file sl_lidar_driver.cpp.
|
inlinestaticprotected |
Definition at line 1416 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get the motor information of the RPLIDAR include the max speed, min speed, desired speed.
motorInfo | The motor information returned from the RPLIDAR |
Implements sl::ILidarDriver.
Definition at line 1023 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Return received scan points even if it's not complete scan
nodebuffer | Buffer provided by the caller application to store the scan data |
count | Once the interface returns, this parameter will store the actual received data count. |
The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call.
Implements sl::ILidarDriver.
Definition at line 962 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1345 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1199 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1364 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Get typical scan mode of lidar.
Implements sl::ILidarDriver.
Definition at line 556 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Wait and grab a complete 0-360 degree scan data previously received. The grabbed scan data returned by this interface always has the following charactistics:
1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 2) All data nodes are belong to exactly ONE complete 360-degrees's scan 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
nodebuffer | Buffer provided by the caller application to store the scan data |
count | The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). Once the interface returns, this parameter will store the actual received data count. |
timeout | Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. |
The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration.
\The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
Implements sl::ILidarDriver.
Definition at line 802 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Wait and grab a complete 0-360 degree scan data previously received with timestamp support.
The returned timestamp belongs to the first data point of the scan data (begining of the scan). Its value is represented based on the current machine's time domain with the unit of microseconds (uS).
If the currently connected LIDAR supports hardware timestamp mechanism, this timestamp will use the actual data emitted by the LIDAR device and remap it to the current machine's time domain.
For other models that do not support hardware timestamps, this data will be deducted through estimation, and there may be a slight deviation from the actual situation.
The grabbed scan data returned by this interface always has the following charactistics:
1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 2) All data nodes are belong to exactly ONE complete 360-degrees's scan 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
nodebuffer | Buffer provided by the caller application to store the scan data |
count | The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). Once the interface returns, this parameter will store the actual received data count. |
timestamp_uS | The reference used to store the timestamp value. |
timeout | Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. |
The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration.
\The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
Implements sl::ILidarDriver.
Definition at line 783 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Check if the connection is established
Implements sl::ILidarDriver.
Definition at line 506 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Ask the LIDAR to use a new baudrate for serial communication The target LIDAR system must support such feature to work. This function does NOT check whether the target LIDAR works with the requiredBaudRate or not. In order to verifiy the result, use getDeviceInfo or other getXXXX functions instead.
requiredBaudRate | The new baudrate required to be used. It MUST matches with the baudrate of the binded channel. |
baudRateDetected | The actual baudrate detected by the LIDAR system |
Implements sl::ILidarDriver.
Definition at line 1058 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Definition at line 1645 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Definition at line 1651 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Implements sl::internal::IProtocolMessageListener.
Definition at line 1655 of file sl_lidar_driver.cpp.
|
inlinestaticprotected |
Definition at line 1394 of file sl_lidar_driver.cpp.
|
inlinestaticprotected |
Definition at line 1382 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Ask the LIDAR core system to reset it self The host system can use the Reset operation to help LIDAR escape the self-protection mode.
timeout | The operation timeout value (in millisecond) |
Implements sl::ILidarDriver.
Definition at line 511 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1215 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Set LPX and S2E series lidar's static IP address
conf | Network parameter that LPX series lidar owned |
timeout | The operation timeout value (in millisecond) for the ethernet udp communication |
Implements sl::ILidarDriver.
Definition at line 887 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Set lidar motor speed The host system can use this operation to set lidar motor speed.
speed | The speed value set to lidar |
Note: The function will stop scan if speed is DEFAULT_MOTOR_SPEED.
Implements sl::ILidarDriver.
Definition at line 968 of file sl_lidar_driver.cpp.
|
inlineprotected |
Definition at line 1158 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Start scan
force | Force the core system to output scan data regardless whether the scanning motor is rotating or not. |
useTypicalScan | Use lidar's typical scan mode or use the compatibility mode (2k sps) |
options | Scan options (please use 0) |
outUsedScanMode | The scan mode selected by lidar |
Implements sl::ILidarDriver.
Definition at line 586 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Start scan in specific mode
force | Force the core system to output scan data regardless whether the scanning motor is rotating or not. |
scanMode | The scan mode id (use getAllSupportedScanModes to get supported modes) |
options | Scan options (please use 0) |
outUsedScanMode | The scan mode selected by lidar |
Implements sl::ILidarDriver.
Definition at line 682 of file sl_lidar_driver.cpp.
|
inline |
Definition at line 664 of file sl_lidar_driver.cpp.
|
inline |
Definition at line 620 of file sl_lidar_driver.cpp.
|
inlinevirtual |
Ask the LIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
timeout | The operation timeout value (in millisecond) for the serial port communication |
Implements sl::ILidarDriver.
Definition at line 763 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1693 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1685 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1677 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1679 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1681 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1691 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1684 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1675 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1689 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1686 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1688 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1694 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1676 of file sl_lidar_driver.cpp.
|
private |
Definition at line 1690 of file sl_lidar_driver.cpp.