ROS——RPLIDAR A1 SDK详解

使用 RPLIDARD 的 SDK 其实重点在于看懂client.cpp和node.cpp两个sample代码，因此在这里我们讲从这里入手学习 RPLIDAR A1 的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该文件中主要定义了函数返回参数的类型，也可以理解为异常参数的类型。

下面分析node.cpp文件
个人觉得先理解以下几个函数比较容易上手

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) {fprintf(stderr, "Error, operation time out.\n");} else {fprintf(stderr, "Error, unexpected error, code: %x\n", 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""Firmware Ver: %d.%02d\n""Hardware Rev: %d\n", devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF, (int)devinfo.hardware_version);return true;
}

下面分析checkRPLIDARHealth函数

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)) { printf("RPLidar health status : %d\n", healthinfo.status);if (healthinfo.status == RPLIDAR_STATUS_ERROR) {fprintf(stderr, "Error, rplidar internal error detected.""Please reboot the device to retry.\n");return false;} else {return true;}} else {fprintf(stderr, "Error, cannot retrieve rplidar health code: %x\n", op_result);return false;}
}

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();;return true;
}

RPLIDAR 停止自传

bool stop_motor(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
{if(!drv)return false;ROS_DEBUG("Stop motor");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;
}

下面分析主函数

int main(int argc, char * argv[]) {ros::init(argc, argv, "rplidar_node");std::string serial_port;//串口名字int serial_baudrate = 115200;//波特率std::string frame_id;bool inverted = false;bool angle_compensate = true;ros::NodeHandle nh;ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);ros::NodeHandle nh_private("~");nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); nh_private.param<int>("serial_baudrate", serial_baudrate, 115200); nh_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, true);printf("RPLIDAR running on ROS package rplidar_ros\n""SDK Version: "RPLIDAR_SDK_VERSION"\n");u_result     op_result;// create the driver instancedrv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);//通过 SDK 创建一个对应的 RPlidarDriver 实例，用来跟RPLIDAR通信if (!drv) {//连接失败fprintf(stderr, "Create Driver fail, exit\n");return -2;}// make connection...if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {//打开串口与APLIDAR连接fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n", 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启动电机旋转drv->startScan();//开启测距扫描ros::Time start_scan_time;//开始扫描时间ros::Time end_scan_time;//结束扫描时间double scan_duration;while (ros::ok()) {rplidar_response_measurement_node_t nodes[360*2];//0~359度，每行代表一个扫描角度的数据size_t   count = _countof(nodes);//一共有多少数据，应该是为了方便后期修改，所以定义了_countof()而不是直接给count赋值start_scan_time = ros::Time::now();//获取扫描开始时间op_result = drv->grabScanData(nodes, count);//grabScanDat（）抓取一圈扫描数据序列end_scan_time = ros::Time::now();//获取扫描结束时间scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3;//计算出扫描时间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_nodes_count = 360;const int angle_compensate_multiple = 1;int angle_compensate_offset = 0;rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));//数据清零int i = 0, j = 0;for( ; i < count; i++ ) {if (nodes[i].distance_q2 != 0) {float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);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,frame_id);} else {int start_node = 0, end_node = 0;int i = 0;// find the first valid node and last valid nodewhile (nodes[i++].distance_q2 == 0);//从头开始寻找第一个不为0的数据start_node = i-1;i = count -1;while (nodes[i--].distance_q2 == 0);//从尾开始寻找第一个不为0的数据end_node = i+1;angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);/*通信协议中写到需要数据需要除以64，RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT=1，在这里右移一位是因为协议中数据是小端存储模式，在angle_q6_checkbit数据存储时最低位是C，即得到数据时需要将angle_q6_checkbit整体右移一位*/angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);//同上publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,start_scan_time, scan_duration, inverted,angle_min, angle_max,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,frame_id);}}ros::spinOnce();}// done!drv->stop();drv->stopMotor();RPlidarDriver::DisposeDriver(drv);return 0;
}

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]);}
}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;
}

ROS——RPLIDAR A1 SDK详解相关推荐

腾讯小程序 java编写_微信小程序使用腾讯地图SDK详解及实现步骤
微信小程序使用腾讯地图SDK详解及实现步骤近期在做一款彩票服务类项目中用到了腾讯地图提供的小程序解决方案,拿来给大家分享一下! 使用起来非常简单,就是一些功能还有待完善. 官方文档:http:// ...
Android高效率编码-第三方SDK详解系列(一)——百度地图，绘制，覆盖物，导航，定位，细腻分解！...
Android高效率编码-第三方SDK详解系列(一)--百度地图,绘制,覆盖物,导航,定位,细腻分解! 这是一个系列,但是我也不确定具体会更新多少期,最近很忙,主要还是效率的问题,所以一些有效的东西还 ...
Android高效率编码-第三方SDK详解系列(二)——Bmob后端云开发，实现登录注册，更改资料，修改密码，邮箱验证，上传，下载，推送消息，缩略图加载等功能
Android高效率编码-第三方SDK详解系列(二)--Bmob后端云开发,实现登录注册,更改资料,修改密码,邮箱验证,上传,下载,推送消息,缩略图加载等功能我的本意是第二篇写Mob的shareSD ...
微信小程序使用腾讯地图SDK详解及实现步骤
信小程序使用腾讯地图SDK详解及实现步骤微信小程序JavaScript SDK: 官方文档:http://lbs.qq.com/qqmap_wx_jssdk/index.html 步骤: 1.申请 ...
Android高效率编码-第三方SDK详解系列(三)——JPush推送牵扯出来的江湖恩怨，XMPP实现推送，自定义客户端推送
Android高效率编码-第三方SDK详解系列(三)--JPush推送牵扯出来的江湖恩怨,XMPP实现推送,自定义客户端推送很久没有更新第三方SDK这个系列了,所以更新一下这几天工作中使用到的推送, ...
Android高效率编码-第三方SDK详解系列(一)——百度地图，绘制，覆盖物，导航，定位，细腻分解！
Android高效率编码-第三方SDK详解系列(一)--百度地图,绘制,覆盖物,导航,定位,细腻分解! 这是一个系列,但是我也不确定具体会更新多少期,最近很忙,主要还是效率的问题,所以一些有效的东西还 ...
直播美颜技术的演进及其应用：直播美颜SDK详解
直播美颜技术的应用,为直播开辟了新的业态,从最初简单的美颜滤镜,到现在的直播美颜SDK,其技术演进历程也是一步步走来. 一.直播美颜技术的演进 1.简单美颜滤镜最初的直播美颜技术,就是通过简单的美颜 ...
i.MX RT开发笔记-02 | i.MX RT1062开发环境搭建（MDK芯片包、NXP SDK详解）
文章目录一.Keil MDK 开发工具 1. 下载安装Keil-MDK 2. MDK芯片包二.MCUXpresso SDK 1. 下载SDK 2. SDK内容 2.1. boards 2.2. C ...
ROS中Remap标签详解
remap标签介绍 remap标签"允许你以更结构化的方式将名称重新映射参数传递给ROS节点,而不是直接设置<节点>的参数属性. 作用重命名一个已经存在的主题. 在自己的lan ...
深入理解ROS技术【3】ROS下的模块详解（129-180）
129 rosgraph rosgraph 包含 rosgraph 命令行工具,可打印有关 ROS 计算图的信息.它还提供了一个可供图形工具使用的内部库. 130 rosgraph_msgs 与 RO ...

ROS——RPLIDAR A1 SDK详解

ROS——RPLIDAR A1 SDK详解相关推荐

最新文章

热门文章