思岚科技RPlidar A3激光雷达ROS源码详解
2024-06-05 04:16:08
**
思岚科技RPlidar A3激光雷达ROS源码详解
**
使用 RPLIDARD 的 SDK 其实重点在于看懂client.cpp和node.cpp两个sample代码，因此在这里我们讲从这里入手学习 RPLIDAR A3 的SDK。
在代码中比较重要的几个文件分别是：
1.rplidar_ros/sdk/include/rplidar_cmd.h该文件中主要定义了与 RPLIDAR 通讯时使用的各种数据的类型。
2.rplidar_ros/sdk/include/rplidar_driver.h该文件中主要声明SDK的各种函数的使用方法以及参数。
3.rplidar_ros/sdk/include/rptypes.h该文件中主要定义了函数返回参数的类型，也可以理解为异常参数的类型。
下面我们先分析头文件：
rplidar.h
//一般情况下开发的项目中仅需要引入该头文件即可使用 RPLIDAR SDK 的所有功能。
//在编程rplidar的相关代码时只需调用rplidar.h即可，但必须清楚其中所包含的其他头文件所含参数的意义，不然无法看懂相关C++代码文件。。。#pragma once//（避免同一个文件被include多次）#include <vector>
#include "hal/types.h" //平台无关的结构和常量定义
#include "rplidar_protocol.h" //定义了 RPLIDAR 通讯协议文档中描述的底层相关数据结构和常量定义。
#include "rplidar_cmd.h" //定义了 RPLIDAR 通讯协议文档中描述的各类请求/应答相关的数据结构和常量定义。#include "rplidar_driver.h"//定义了 SDK 核心驱动接口: RPlidarDriver 的类声明。#define RPLIDAR_SDK_VERSION "1.9.0"//code版本
rplidar_cmd.h
//定义了 RPLIDAR 通讯协议文档中描述的各类请求/应答相关的数据结构和常量定义。#pragma once#include "rplidar_protocol.h"// Commands
//-----------------------------------------// Commands without payload and response负载和响应
#define RPLIDAR_CMD_STOP 0x25//离开扫描模式，进入空闲状态
#define RPLIDAR_CMD_SCAN 0x20//请求进入扫描采样状态
#define RPLIDAR_CMD_FORCE_SCAN 0x21//请求进入扫描采样状态，强制数据输出
#define RPLIDAR_CMD_RESET 0x40//测距核心软重启// Commands without payload but have response
#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50//获取设备序列号等信息
#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52//获取设备健康状态#define RPLIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17获取单次激光测距的用时#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8// Commands with payload and have response
#define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17请求进入扫描状态，幷工作在最高采样频率下
#define RPLIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24
#define RPLIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24
#define RPLIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24
//add for A2 to set RPLIDAR motor pwm（脉宽调变）when using accessory board（开发系统)
#define RPLIDAR_CMD_SET_MOTOR_PWM 0xF0// 设置PWM
#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF//系统标志#if defined(_WIN32) //如果是win32下编译则执行
#pragma pack(1)//结构体边界设置为1字节，也就是说储存在内存中是连续的
#endif// Payloads
// ------------------------------------------
#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0 // (正常模式）
#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail（固定角度）
//for express working flag(extending express scan protocol)
#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001
#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002//for ultra express working flag
#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001
#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002#define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0)
#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1)
#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2)typedef struct _rplidar_payload_express_scan_t {//带有负载的数据包_u8 working_mode;_u16 working_flags;_u16 param;
} __attribute__((packed)) rplidar_payload_express_scan_t;typedef struct _rplidar_payload_hq_scan_t {_u8 flag;_u8 reserved[32];
} __attribute__((packed)) rplidar_payload_hq_scan_t;typedef struct _rplidar_payload_get_scan_conf_t {_u32 type;_u8 reserved[32];
} __attribute__((packed)) rplidar_payload_get_scan_conf_t;
#define MAX_MOTOR_PWM 1023//电机脉宽最大值
#define DEFAULT_MOTOR_PWM 660//电机脉宽默认值
//电机PWM结构
typedef struct _rplidar_payload_motor_pwm_t {_u16 pwm_value;
} __attribute__((packed)) rplidar_payload_motor_pwm_t;
//开发系统标志
typedef struct _rplidar_payload_acc_board_flag_t {_u32 reserved;
} __attribute__((packed)) rplidar_payload_acc_board_flag_t;// Response
// ------------------------------------------
#define RPLIDAR_ANS_TYPE_DEVINFO 0x4//设备信心获取 起始应答结尾
#define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6//设备健康状态获取 起始应答结尾#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81//普通采集
// Added in FW ver 1.17
#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82//高速采集
#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83// Added in FW ver 1.17
#define RPLIDAR_ANS_TYPE_SAMPLE_RATE 0x15
//added in FW ver 1.23alpha
#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84
//added in FW ver 1.24
#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20
#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1)
typedef struct _rplidar_response_acc_board_flag_t {_u32 support_flag;
} __attribute__((packed)) rplidar_response_acc_board_flag_t;//设备状态
#define RPLIDAR_STATUS_OK 0x0
#define RPLIDAR_STATUS_WARNING 0x1
#define RPLIDAR_STATUS_ERROR 0x2#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0)
#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0)#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0)
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1//激光测距用时获取数据包typedef struct _rplidar_response_sample_rate_t {_u16 std_sample_duration_us;//在标准扫描模式下，设备测距核心进行单次激光测距的耗时 单位：微秒_u16 express_sample_duration_us;//在高速采样模式下，设备测距核心进行单次激光测距的耗时 单位：微秒
} __attribute__((packed)) rplidar_response_sample_rate_t;//激光测距测量节点数据包
typedef struct _rplidar_response_measurement_node_t {_u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6;信号质量_u16 angle_q6_checkbit; // check_bit:1;angle_q6:15;测距点相对于RPLIDAR朝向夹角 实际角度 = angle_q6 / 64.0 Deg_u16 distance_q2;//测距点相对于RPLIDAR的距离 实际距离 = distance_q2 / 4.0 mm
} __attribute__((packed)) rplidar_response_measurement_node_t;//[distance_sync flags]
#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3)
#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC)// 激光测距小屋测量节点数据包
typedef struct _rplidar_response_cabin_nodes_t {_u16 distance_angle_1; // see [distance_sync flags]_u16 distance_angle_2; // see [distance_sync flags]_u8 offset_angles_q3;
} __attribute__((packed)) rplidar_response_cabin_nodes_t; #define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15)// 激光测距容器测量节点数据包
typedef struct _rplidar_response_capsule_measurement_nodes_t {_u8 s_checksum_1; // see [s_checksum_1]_u8 s_checksum_2; // see [s_checksum_1]_u16 start_angle_sync_q6;rplidar_response_cabin_nodes_t cabins[16];
} __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t;
// ext1 : x2 boost mode#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12
#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10typedef struct _rplidar_response_ultra_cabin_nodes_t {// 31 0// | predict2 10bit | predict1 10bit | major 12bit |_u32 combined_x3;
} __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t; typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t {_u8 s_checksum_1; // see [s_checksum_1]_u8 s_checksum_2; // see [s_checksum_1]_u16 start_angle_sync_q6;rplidar_response_ultra_cabin_nodes_t ultra_cabins[32];
} __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t;typedef struct rplidar_response_measurement_node_hq_t {_u16 angle_z_q14; _u32 dist_mm_q2; _u8 quality; _u8 flag;
} __attribute__((packed)) rplidar_response_measurement_node_hq_t;typedef struct _rplidar_response_hq_capsule_measurement_nodes_t{_u8 sync_byte;_u64 time_stamp;rplidar_response_measurement_node_hq_t node_hq[16];_u32 crc32;
}__attribute__((packed)) rplidar_response_hq_capsule_measurement_nodes_t;# define RPLIDAR_CONF_SCAN_COMMAND_STD 0
# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS 1
# define RPLIDAR_CONF_SCAN_COMMAND_HQ 2
# define RPLIDAR_CONF_SCAN_COMMAND_BOOST 3
# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY 4
# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5#define RPLIDAR_CONF_ANGLE_RANGE 0x00000000
#define RPLIDAR_CONF_DESIRED_ROT_FREQ 0x00000001
#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002
#define RPLIDAR_CONF_MIN_ROT_FREQ 0x00000004
#define RPLIDAR_CONF_MAX_ROT_FREQ 0x00000005
#define RPLIDAR_CONF_MAX_DISTANCE 0x00000060#define RPLIDAR_CONF_SCAN_MODE_COUNT 0x00000070
#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071
#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074
#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075
#define RPLIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C
#define RPLIDAR_CONF_SCAN_MODE_NAME 0x0000007F
#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4
#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5typedef struct _rplidar_response_get_lidar_conf{_u32 type;_u8 payload[0];
}__attribute__((packed)) rplidar_response_get_lidar_conf_t;typedef struct _rplidar_response_set_lidar_conf{_u32 result;
}__attribute__((packed)) rplidar_response_set_lidar_conf_t;typedef struct _rplidar_response_device_info_t {_u8 model;_u16 firmware_version;_u8 hardware_version;_u8 serialnum[16];
} __attribute__((packed)) rplidar_response_device_info_t;//设备信息数据报文
typedef struct _rplidar_response_device_health_t {_u8 status;_u16 error_code;
} __attribute__((packed)) rplidar_response_device_health_t;// Definition of the variable bit scale encoding mechanism
#define RPLIDAR_VARBITSCALE_X2_SRC_BIT 9
#define RPLIDAR_VARBITSCALE_X4_SRC_BIT 11
#define RPLIDAR_VARBITSCALE_X8_SRC_BIT 12
#define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14#define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512
#define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280
#define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792
#define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \( (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1)#if defined(_WIN32)
#pragma pack()
#endif
rplidar_driver.h
#pragma once//（避免同一个文件被include多次）#ifndef __cplusplus
#error "The RPlidar SDK requires a C++ compiler to be built"
#endif#ifndef DEPRECATED#ifdef __GNUC__#define DEPRECATED(func) func __attribute__ ((deprecated))#elif defined(_MSC_VER)#define DEPRECATED(func) __declspec(deprecated) func#else#pragma message("WARNING: You need to implement DEPRECATED for this compiler")#define DEPRECATED(func) func#endif
#endifnamespace rp { namespace standalone{ namespace rplidar {struct RplidarScanMode {_u16 id;float us_per_sample; // microseconds per samplefloat max_distance; // max distance_u8 ans_type; // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT*char scan_mode[64]; // name of scan mode, max 63 characters
};enum {DRIVER_TYPE_SERIALPORT = 0x0,//枚举量驱动程序类型串口为0x0（linux被驱动使用的连接类型）DRIVER_TYPE_TCP = 0x1,
};class ChannelDevice
{public:virtual bool bind(const char*, uint32_t ) = 0;virtual bool open() {return true;}virtual void close() = 0;virtual void flush() {return;}virtual bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;virtual int senddata(const _u8 * data, size_t size) = 0;virtual int recvdata(unsigned char * data, size_t size) = 0;virtual void setDTR() {return;}virtual void clearDTR() {return;}virtual void ReleaseRxTx() {return;}
};class RPlidarDriver {public:enum {DEFAULT_TIMEOUT = 2000, //2000 ms,枚举量默认超时时间2000 ms};enum {MAX_SCAN_NODES = 8192,};enum {LEGACY_SAMPLE_DURATION = 476,};public:/// Create an RPLIDAR Driver Instance（实例）/// This interface（接口） should be invoked（调用） first before any other operations（操作）////// \param（参数） drivertype（驱动类型） the connection type used by the driver. static RPlidarDriver * CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT);//静态创建对象接口/// Dispose the RPLIDAR Driver Instance specified by the drv parameter/// Applications should invoke this interface when the driver instance is no longer used in order to free memorystatic void DisposeDriver(RPlidarDriver * drv);//处理驱动程序，静态接口函数析构RPlidar实例/// Open the specified serial port and connect to a target RPLIDAR device////// \param port_path the device path of the serial port /// e.g. on Windows, it may be com3 or \\.\com10 /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc////// \param baudrate （波特率） the baudrate used/// For most RPLIDAR models, the baudrate should be set to 115200////// \param flag（标志） other flags/// Reserved for future use, always set to Zerovirtual u_result connect(const char *, _u32, _u32 flag = 0) = 0;//设备连接函数,调用时会默认停止电机旋转，测试时需要使用startMotor启动电机旋转/// Disconnect with the RPLIDAR and close the serial port（断开连接并关上串口端）virtual void disconnect() = 0;//断开连接/// Returns TRUE when the connection has been established（连接已建立）virtual bool isConnected() = 0;//建立连接/// Ask the RPLIDAR core system to reset it self/// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode.////// \param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0;//超时重置// FW1.24/// Get all scan modes that supported by lidarvirtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;/// Get typical scan mode of lidarvirtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;/// Start scan////// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not./// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps)/// \param options Scan options (please use 0)/// \param outUsedScanMode The scan mode selected by lidarvirtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0;/// Start scan in specific mode////// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not./// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes)/// \param options Scan options (please use 0)/// \param outUsedScanMode The scan mode selected by lidarvirtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT) = 0;/// Retrieve（恢复） the health status of the RPLIDAR/// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.////// \param health The health status info returned from the RPLIDAR////// \param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0;//获取 RPLIDAR 设备的健康状态/// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc./// /// \param info The device information returned from the RPLIDAR/// \param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0;//获取 RPLIDAR 设备序列号、固件版本等信息/// Get the sample（样本） duration（持续时间） information of the RPLIDAR./// DEPRECATED, please use RplidarScanMode::us_per_sample////// \param rateInfo The sample duration information returned from the RPLIDAR/// \param timeout The operation timeout value (in millisecond) for the serial port communicationDEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT)) = 0;//获取 RPLIDAR 分别在标准 Scan 以及 ExpressScan 模式下单次激光采样的用时。 单位为微秒(uS)/// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only./// /// \param pwm The motor pwm value would like to set virtual u_result setMotorPWM(_u16 pwm) = 0;//向开发套件的 USB 附件板发送特定的 PWM 占空比,控制RPLIDAR 扫描电机转速/// Start RPLIDAR's motor when using accessory（附件） boardvirtual u_result startMotor() = 0;//启动电机/// Stop RPLIDAR's motor when using accessory boardvirtual u_result stopMotor() = 0;//停止电机/// Check whether the device support motor control./// Note: this API will disable grab./// /// \param support Return the result./// \param timeout The operation timeout value (in millisecond) for the serial port communication. virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0;//检测附件板是否支持电机 PWM 控制。/// Calculate RPLIDAR's current scanning frequency from the given scan data/// DEPRECATED, please use getFrequency(RplidarScanMode, size_t)////// 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////// \param inExpressMode Indicate whether the RPLIDAR is in express mode（表达模式）/// \param count The number of sample nodes inside the given buffer（缓冲区）/// \param frequency The scanning frequency (in HZ) calcuated by the interface./// \param is4kmode Return whether the RPLIDAR is working on 4k sample rate mode.DEPRECATED(virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)) = 0;//从实际抓取的一圈扫描数据序列计算 RPLIDAR 的转速/// Calculate RPLIDAR'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////// \param scanMode Lidar's current scan mode/// \param count The number of sample nodes inside the given buffervirtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) = 0;/// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode)/// A background thread（后台线程） will be created by the RPLIDAR driver to fetch（获取） the scan data continuously./// User Application（应用） can use the grabScanData() interface to retrieved（恢复） the scan data cached（隐藏） previous by this background thread.////// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not.（旋转与否）/// \param timeout The operation timeout value (in millisecond) for the serial port communication.virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0;/// Check whether the device support express mode./// DEPRECATED, please use getAllSupportedScanModes////// \param support Return the result./// \param timeout The operation timeout value (in millisecond) for the serial port communication.DEPRECATED(virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT)) = 0;/// Ask the RPLIDAR core system to stop the current scan operation and enter idle state（空闲状态）. The background thread will be terminated（终止）////// \param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0;/// Wait and grab a complete 0-360 degree scan data previously received. /// NOTE: This method only support distance less than 16.38 meters, for longer distance, please use grabScanDataHq/// 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.////// \param nodebuffer（存储扫描数据的缓冲区 ） Buffer provided by the caller application to store the scan data////// \param 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.////// \param 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 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.DEPRECATED(virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT)) = 0;/// 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.////// \param nodebuffer Buffer provided by the caller application to store the scan data////// \param 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.////// \param 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 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.virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0;/// Ascending the scan data according to the angle value in the scan.////// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData////// \param 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 RESULT_OPERATION_FAIL when all the scan data is invalid. DEPRECATED(virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)) = 0;/// Ascending the scan data according to the angle value in the scan.////// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData////// \param 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 RESULT_OPERATION_FAIL when all the scan data is invalid. virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) = 0;/// Return received scan points even if it's not complete scan////// \param nodebuffer Buffer provided by the caller application to store the scan data////// \param count Once the interface returns, this parameter will store the actual received data count.////// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0;/// Return received scan points even if it's not complete scan////// \param nodebuffer Buffer provided by the caller application to store the scan data////// \param count Once the interface returns, this parameter will store the actual received data count.////// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0;virtual ~RPlidarDriver() {}//此函数将在源文件中使用但不在此类中使用
protected:RPlidarDriver(){}public:ChannelDevice* _chanDev;
};}}}
rplidar_protocol.h
//定义了 RPLIDAR 通讯协议文档中描述的底层相关数据结构和常量定义。#pragma once//（避免同一个文件被include多次）// RP-Lidar Input Packets（输入数据包）#define RPLIDAR_CMD_SYNC_BYTE 0xA5//(十六进制0x开头）请求报文开始字节
#define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80//请求报文负载#define RPLIDAR_ANS_SYNC_BYTE1 0xA5//应答报文起始标志1
#define RPLIDAR_ANS_SYNC_BYTE2 0x5A//应答报文起始标志2#define RPLIDAR_ANS_PKTFLAG_LOOP 0x1//多次应答模式#define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF
#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30)#if defined(_WIN32)
#pragma pack(1)//每个结构成员都必须限制在1个字节之内，也就是说储存在内存中是连续的
#endif
//请求报文格式
typedef struct _rplidar_cmd_packet_t {_u8 syncByte;//must be RPLIDAR_CMD_SYNC_BYTE//起始标志_u8 cmd_flag;//请求命令_u8 size;//负载长度_u8 data[0];//请求负载数据
} __attribute__((packed)) rplidar_cmd_packet_t;//__attribute__ ((packed)) 的作用就是告诉编译器取消结构在编译过程中的优化对齐,按照实际占用字节数进行对齐
//使用之后才会是几字节就是几字节，否则就每个变量的字节长度都跟结构体中变量字节长度最长相同 //应答报文格式typedef struct _rplidar_ans_header_t {_u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1（起始标志1）_u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2（起始标志2）_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2;（应答报文长度）_u8 type;//应答模式
} __attribute__((packed)) rplidar_ans_header_t;#if defined(_WIN32)
#pragma pack()
#endif
//按照实际占用字节数进行对齐
rptypes.h
//平台无关的结构和常量定义#pragma once#ifdef _WIN32//fake stdint.h for VC onlytypedef signed char int8_t;
typedef unsigned char uint8_t;typedef __int16 int16_t;
typedef unsigned __int16 uint16_t;typedef __int32 int32_t;
typedef unsigned __int32 uint32_t;typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;#else#include <stdint.h>//定义int8 uint8等类型#endif//based on stdint.h
typedef int8_t _s8;
typedef uint8_t _u8;typedef int16_t _s16;
typedef uint16_t _u16;typedef int32_t _s32;
typedef uint32_t _u32;typedef int64_t _s64;
typedef uint64_t _u64;#define __small_endian#ifndef __GNUC__
#define __attribute__(x)
#endif// The _word_size_t uses actual data bus width of the current CPU
#ifdef _AVR_
typedef _u8 _word_size_t;
#define THREAD_PROC
#elif defined (WIN64)
typedef _u64 _word_size_t;
#define THREAD_PROC __stdcall
#elif defined (WIN32)
typedef _u32 _word_size_t;
#define THREAD_PROC __stdcall
#elif defined (__GNUC__)
typedef unsigned long _word_size_t;
#define THREAD_PROC
#elif defined (__ICCARM__)
typedef _u32 _word_size_t;
#define THREAD_PROC
#endiftypedef uint32_t u_result;#define RESULT_OK 0
#define RESULT_FAIL_BIT 0x80000000
#define RESULT_ALREADY_DONE 0x20
#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT)
#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT)
#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT)
#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT)#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 )
#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) )typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );
node.cpp
/*
Sensor_msgs/LaserScan:
header(stamp[第一条射线的获得时间]+frame_id[围绕z轴测量的角度])
angle_min(起始角度)
angle_max(结束角度)
angle_increment(角距离)
time_increment(测量时间)
scan_time(扫描时间)
range_min(有效范围最小值)
range_max(有效范围最大值)
ranges(有效范围)
intensities(强度)
*/#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_srvs/Empty.h"
#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
//避免源库中没有_countof函数，定义此函数为返回数组元素个数#define DEG2RAD(x) ((x)*M_PI/180.)//角度弧度信息转换using namespace rp::standalone::rplidar;RPlidarDriver * drv = NULL;//RPlidarDriver是在rplidar_driver.h头文件中定义好的一个类//雷达发布消息子函数
void publish_scan(ros::Publisher *pub,//对象rplidar_response_measurement_node_hq_t *nodes,//雷达数据（该数据类型在rplidarcmd.h被定义，包含sync_quality，angle_q6_checkbit，distance_q2）size_t node_count, ros::Time start,//次数(size_t是标准C库中定义的，应为unsigned int，在64位系统中为 long unsigned int)，起始时间(该数据类型在ros.h中被定义为获取时间）double scan_time, bool inverted,//扫描时间，是否转换float angle_min, float angle_max,//角度范围float max_distance,//最大距离std::string frame_id)//id
{static int scan_count = 0;//定义一个计数变量sensor_msgs::LaserScan scan_msg;//用ros已有的雷达数据模型定义扫描所发出的消息scan_msg.header.stamp = start;//扫描起始时间scan_msg.header.frame_id = frame_id;//序列idscan_count++;//计数bool reversed = (angle_max > angle_min);if ( reversed ) {scan_msg.angle_min = M_PI - angle_max;//M_PI为3.1415926，即180度scan_msg.angle_max = M_PI - angle_min;} else {scan_msg.angle_min = M_PI - angle_min;scan_msg.angle_max = M_PI - angle_max;}
//角度修正，从小到大scan_msg.angle_increment =(scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1);//扫描精度scan_msg.scan_time = scan_time;//扫描时间开始scan_msg.time_increment = scan_time / (double)(node_count-1);//时间间隔scan_msg.range_min = 0.15;//最小的范围scan_msg.range_max = max_distance;//8.0;//最大的范围scan_msg.intensities.resize(node_count);//信号质量scan_msg.ranges.resize(node_count);//范围bool reverse_data = (!inverted && reversed) || (inverted && !reversed);if (!reverse_data) {for (size_t i = 0; i < node_count; i++) {float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;if (read_value == 0.0)scan_msg.ranges[i] = std::numeric_limits<float>::infinity();//返回编译器允许的float类型无穷大elsescan_msg.ranges[i] = read_value;scan_msg.intensities[i] = (float) (nodes[i].quality >> 2);}} else {for (size_t i = 0; i < node_count; i++) {float read_value = (float)nodes[i].dist_mm_q2/4.0f/1000;//距离信息if (read_value == 0.0)scan_msg.ranges[node_count-1-i] = std::numeric_limits<float>::infinity();elsescan_msg.ranges[node_count-1-i] = read_value;scan_msg.intensities[node_count-1-i] = (float) (nodes[i].quality >> 2);//信号质量信息}}pub->publish(scan_msg);
}bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{u_result op_result;rplidar_response_device_info_t devinfo;/*
在rplida_cmd.h文件中可以找到rplidar_response_device_info_t定义如下:
typedef struct _rplidar_response_device_info_t {_u8 model;_u16 firmware_version;_u8 hardware_version;_u8 serialnum[16];
} __attribute__((packed)) rplidar_response_device_info_t;
从上面的定义可以看出，在这里定义了设备信息，包括序列号，固件版本，设备型号等。
*/op_result = drv->getDeviceInfo(devinfo);/*
在rplida_driver.h文件中可以找到getDeviceInfo函数的使用说明:
Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.\param info The device information returned from the RPLIDAR\param timeout The operation timeout value (in millisecond) for the serial port communication
virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0;
*/if (IS_FAIL(op_result)) {//通过函数返回值判断是否正常if (op_result == RESULT_OPERATION_TIMEOUT) {ROS_ERROR("Error, operation time out. RESULT_OPERATION_TIMEOUT! ");} else {ROS_ERROR("Error, unexpected error, code: %x",op_result);}return false;}// print out the device serial number, firmware and hardware version number..printf("RPLIDAR S/N: ");for (int pos = 0; pos < 16 ;++pos) {printf("%02X", devinfo.serialnum[pos]);}//输出设备信息printf("\n");ROS_INFO("Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF);ROS_INFO("Hardware Rev: %d",(int)devinfo.hardware_version);return true;
}//检查rplidar设备健康信息
bool checkRPLIDARHealth(RPlidarDriver * drv)
{u_result op_result;//函数返回值rplidar_response_device_health_t healthinfo;/*
在rplida_cmd.h文件中可以找到rplidar_response_device_health_t定义如下:
typedef struct _rplidar_response_device_health_t {_u8 status;//从RPLIDAR返回的运行状况信息_u16 error_code;//错误信息
} __attribute__((packed)) rplidar_response_device_health_t;
*/op_result = drv->getHealth(healthinfo);/*
在rplida_driver.h文件中可以找到getHealth函数的使用说明:
Retrieve the health status of the RPLIDAR
The host system can use this operation to check whether RPLIDAR is in the self-protection mode.\param health The health status info returned from the RPLIDAR\param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0;
*/if (IS_OK(op_result)) { ROS_INFO("RPLidar health status : %d", healthinfo.status);if (healthinfo.status == RPLIDAR_STATUS_ERROR) {ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry.");return false;} else {return true;}} else {ROS_ERROR("Error, cannot retrieve rplidar health code: %x", op_result);return false;}
}//RPLIDAR 停止自传
bool stop_motor(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)//创建一个服务
{if(!drv)return false;ROS_DEBUG("Stop motor");//ROS_DEBUG的信息在程序运行时不直接显示的Terminal,须利用rxconsole查看，一些调试中需要观察的信息用ROS_DEBUG显示drv->stop();/*在rplidar_driver.h中可以找到stop()函数的定义Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated\param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0;*/drv->stopMotor();/*在rplidar_driver.h中可以找到stopMotor()函数的定义Stop RPLIDAR's motor when using accessory boardvirtual u_result stopMotor() = 0;
*/return true;
}//RPLIDAR 开启自传（电机启动通过皮带带动激光雷达转动）
bool start_motor(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
{if(!drv)return false;ROS_DEBUG("Start motor");drv->startMotor();/*在rplidar_driver.h中可以找到startMotor()函数的定义Start RPLIDAR's motor when using accessory boardvirtual u_result startMotor() = 0;
*/drv->startScan(0,1);return true;
}static float getAngle(const rplidar_response_measurement_node_hq_t& node)
{return node.angle_z_q14 * 90.f / 16384.f;
}int main(int argc, char * argv[]) {ros::init(argc, argv, "rplidar_node");std::string serial_port;//串口名字int serial_baudrate = 115200;//波特率std::string frame_id;//idbool inverted = false;//是否转换标志位bool angle_compensate = true;//角度补偿标志位float max_distance = 8.0;int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degreestd::string scan_mode;ros::NodeHandle nh;//下面是设置节点进程的句柄ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);//将节点设置成发布者，并将所发布的主题和类型的名称告知节点管理器。第一个参数是缓冲区大小，名为scan，如果主题发布速度较快，则将缓冲区设置成1000个消息）ros::NodeHandle nh_private("~");//launch可以进行一些初始化nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); nh_private.param<int>("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3nh_private.param<std::string>("frame_id", frame_id, "laser_frame");nh_private.param<bool>("inverted", inverted, false);nh_private.param<bool>("angle_compensate", angle_compensate, false);nh_private.param<std::string>("scan_mode", scan_mode, std::string());ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");u_result op_result;// create the driver instance创建静态接口drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);//通过 SDK 创建一个对应的 RPlidarDriver 实例，用来跟RPLIDAR通信if (!drv) {//连接失败，创建失败退出，返回-2ROS_ERROR("Create Driver fail, exit");return -2;}// make connection...建立链接，失败返回-1if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {//打开串口与RPLIDAR连接ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());RPlidarDriver::DisposeDriver(drv);return -1;}// get rplidar device infoif (!getRPLIDARDeviceInfo(drv)) {return -1;}// check health...检查设备是否健康if (!checkRPLIDARHealth(drv)) {RPlidarDriver::DisposeDriver(drv);return -1;}//添加回调函数ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor);drv->startMotor();//默认使用是能DTR启动电机旋转RplidarScanMode current_scan_mode;if (scan_mode.empty()) {op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, ¤t_scan_mode);} else {std::vector<RplidarScanMode> allSupportedScanModes;op_result = drv->getAllSupportedScanModes(allSupportedScanModes);if (IS_OK(op_result)) {_u16 selectedScanMode = _u16(-1);for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {if (iter->scan_mode == scan_mode) {selectedScanMode = iter->id;break;}}if (selectedScanMode == _u16(-1)) {ROS_ERROR("scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str());for (std::vector<RplidarScanMode>::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) {ROS_ERROR("\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode,iter->max_distance, (1000/iter->us_per_sample));}op_result = RESULT_OPERATION_FAIL;} else {op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, ¤t_scan_mode);}}}if(IS_OK(op_result)){//default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per usangle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0);if(angle_compensate_multiple < 1) angle_compensate_multiple = 1;max_distance = current_scan_mode.max_distance;ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d", current_scan_mode.scan_mode,current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample), angle_compensate_multiple);}else{ROS_ERROR("Can not start scan: %08x!", op_result);}//记录扫描起始时间，持续时间ros::Time start_scan_time;ros::Time end_scan_time;double scan_duration;while (ros::ok()) {//储存信号质量，角度信息，距离信息的变量rplidar_response_measurement_node_hq_t nodes[360*8];//0~359度，每行代表一个扫描角度的数据size_t count = _countof(nodes);//得到数组长度，一共有多少数据，应该是为了方便后期修改，所以定义了_countof()而不是直接给count赋值//得到扫描时间，雷达数据start_scan_time = ros::Time::now();//获取扫描开始时间op_result = drv->grabScanDataHq(nodes, count);//grabScanDat（）抓取一圈扫描数据序列end_scan_time = ros::Time::now();//获取扫描结束时间scan_duration = (end_scan_time - start_scan_time).toSec();//计算出扫描时间//成功扫描if (op_result == RESULT_OK) {//得到排序后的雷达数据op_result = drv->ascendScanData(nodes, count);//ascendScanData()将扫描到的数据按照角度递增排序float angle_min = DEG2RAD(0.0f);//扫描角度最小值float angle_max = DEG2RAD(359.0f);//扫描角度最大值if (op_result == RESULT_OK) {//成功获取到扫描一周的数据
//进行角度补偿if (angle_compensate) {//const int angle_compensate_multiple = 1;const int angle_compensate_nodes_count = 360*angle_compensate_multiple;int angle_compensate_offset = 0;
//初始化，清0rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count];memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_hq_t));int i = 0, j = 0;for( ; i < count; i++ ) {//距离不是0if (nodes[i].dist_mm_q2 != 0) {float angle = getAngle(nodes[i]);int angle_value = (int)(angle * angle_compensate_multiple);
//计算当前角度，如果角度比上次小则，记录if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
//只修正一个for (j = 0; j < angle_compensate_multiple; j++) {angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];}}}publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,start_scan_time, scan_duration, inverted,angle_min, angle_max, max_distance,frame_id);} else {int start_node = 0, end_node = 0;int i = 0;// find the first valid node and last valid nodewhile (nodes[i++].dist_mm_q2 == 0);//从头开始寻找第一个不为0的数据start_node = i-1;i = count -1;while (nodes[i--].dist_mm_q2 == 0);//从尾开始寻找第一个不为0的数据end_node = i+1;angle_min = DEG2RAD(getAngle(nodes[start_node]));angle_max = DEG2RAD(getAngle(nodes[end_node]));publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,start_scan_time, scan_duration, inverted,angle_min, angle_max, max_distance,frame_id);}} else if (op_result == RESULT_OPERATION_FAIL) {// All the data is invalid, just publish themfloat angle_min = DEG2RAD(0.0f);float angle_max = DEG2RAD(359.0f);publish_scan(&scan_pub, nodes, count,start_scan_time, scan_duration, inverted,angle_min, angle_max, max_distance,frame_id);}}ros::spinOnce();//如果有一个订阅者出现，ROS就会更新并读取所有主题}// done!drv->stop();drv->stopMotor();RPlidarDriver::DisposeDriver(drv);return 0;
}
client.cpp
//client.cpp文件订阅node.cpp中接收的数据。可能会有朋友问为何没有直接在node.cpp中直接将读到的数据输出，个人感觉应该是为了体现ROS发布订阅的特点，同时方便移植，如果想在其他节点使用数据，只需要订阅node中的数据即可。#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"#define RAD2DEG(x) ((x)*180./M_PI) //弧度制数据转换为角度void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)//sensor_msgs命名空间下，LaserScan结构体里面的
{int count = scan->scan_time / scan->time_increment;ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));for(int i = 0; i < count; i++) {float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
/**open()打开一个.txt，然后直接写入可保存数据信息。ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);这个里面就是角度和距离。
*/ }
}int main(int argc, char **argv)
{ros::init(argc, argv, "rplidar_node_client");ros::NodeHandle n;ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);ros::spin();return 0;
}
思岚科技RPlidar A3激光雷达ROS源码详解相关推荐
- 思岚科技Rplidar A3实现指定角度扫描及扫描结果存储输出
思岚科技Rplidar A3实现指定任意角度扫描及扫描结果.txt存储输出 前言 思岚科技Rplidar系列产品非常不错,拥有A1.A2.A3系列成熟的商业激光雷达产品.产品均可从官网获取相应的SDK ...
- 激光雷达初体验 - Ubuntu 18.04 + 思岚科技 RPLIDAR A1M8 + ROS 上手使用
思岚科技 RPLIDAR A1M8 + ROS 上手使用 一.开箱图 二.文档和SDK下载 `官网链接` 三.SDK 安装 四.SDK 使用 五.小结 一.开箱图 型号 RPLIDAR A1 配料 激 ...
- Rocksdb Compaction源码详解（二）：Compaction 完整实现过程 概览
文章目录 1. 摘要 2. Compaction 概述 3. 实现 3.1 Prepare keys 过程 3.1.1 compaction触发的条件 3.1.2 compaction 的文件筛选过程 ...
- webbench1.5源码详解
webbench1.5源码详解 前言 Webbench是Linux下的一个网站压力测试工具,它是由Lionbridge公司(http://www.lionbridge.com)开发. ...
- 【Live555】live555源码详解（九）：ServerMediaSession、ServerMediaSubsession、live555MediaServer
[Live555]live555源码详解系列笔记 继承协作关系图 下面红色表示本博客将要介绍的三个类所在的位置: ServerMediaSession.ServerMediaSubsession.Dy ...
- 【Live555】live555源码详解系列笔记
[Live555]liveMedia下载.配置.编译.安装.基本概念 [Live555]live555源码详解(一):BasicUsageEnvironment.UsageEnvironment [L ...
- 【Live555】live555源码详解（八）：testRTSPClient
[Live555]live555源码详解系列笔记 继承协作关系图 下面红色表示本博客将要介绍的testRTSPClient实现的三个类所在的位置: ourRTSPClient.StreamClient ...
- 【Live555】live555源码详解（七）：GenericMediaServer、RTSPServer、RTSPClient
[Live555]live555源码详解系列笔记 继承协作关系图 下面红色表示本博客将要介绍的三个类所在的位置: GenericMediaServer.RTSPServer.RTSPClient 14 ...
- 【Live555】live555源码详解（六）：FramedSource、RTPSource、RTPSink
[Live555]live555源码详解系列笔记 继承协作关系图 下面红色表示本博客将要介绍的三个类所在的位置: FramedSource.RTPSource.RTPSink 11.FramedSou ...
- 【Live555】live555源码详解（五）：MediaSource、MediaSink、MediaSession、MediaSubsession
[Live555]live555源码详解系列笔记 继承协作关系图 下面红色表示本博客将要介绍的四个类所在的位置: MediaSource.MediaSink.MediaSession.MediaSub ...
最新文章
- IO模型(select, poll, epoll的区别和原理)
- 1100 Mars Numbers (20 分)【难度: 一般 / 知识点: 模拟】
- 文献记录(part66)--一种基于交叉熵的社区发现算法
- [转]十五分钟介绍 Redis数据结构
- 关于 libpcap的安装
- delphi 到出execl2010 文件损坏_如何修复Linux中损坏的软件包？
- log4net配置文件样本
- 输出素数和排序后的数组和杨辉三角
- SIP协议搭建电信级VOIP/IM运营平台--架构篇（sip集群）
- phpMyAdmin view_create.php 跨站脚本漏洞
- matlab鼠标中键不能放大缩小,proe鼠标中键不能缩放如何解决
- 打开其他软件时，老是弹出Xftp7安装的问题（msi报错）
- vue大屏项目开发框架dataV
- 人人商城(分销版)1.11.7微擎原版，装修店铺后，网站链接失效，页面不显示数据
- 程序员520告白Html+Js+Css花瓣相册网页模板❤程序员表白必备
- LaTex特殊字符和符号
- python扫描局域网端口和ip_我想用python扫描局域网内的ip和mac地址，需要什么第三方库...
- Spring boot集成Redis实现sessions共享时，sessions过期时间问题分析
- IP核Map编译报错：Buffers of the same direction cannot beplaced in series.
- excel计算二元线性回归_用Excel做回归分析