需求分析

rplidar A1和rplidar A2使用相同的接口和SDK，因此可以均可以使用该代码。官方的DEMO实现了激光雷达的二维扫描，并将扫描结果事实显示在rviz中，可以满足基本的需求。官方DEMO中将扫描结果在rviz的“LaserScan”中显示，但是由于“LaserScan”只能显示二维坐标，所以当需要使用二维激光雷达三维建模时官方DEMO便不能使用，因此我将其移植到rviz的“PointCloud2”中显示。

消息类型查看

输入以下命令查询“LaserScan”参数：

rosmsg show sensor_msgs/LaserScan

显示如下

std_msgs/Header headeruint32 seqtime stampstring frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

输入以下命令查询“PointCloud2”参数：

rosmsg show sensor_msgs/PointCloud2

显示如下

std_msgs/Header headeruint32 seqtime stampstring frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fieldsuint8 INT8=1uint8 UINT8=2uint8 INT16=3uint8 UINT16=4uint8 INT32=5uint8 UINT32=6uint8 FLOAT32=7uint8 FLOAT64=8string nameuint32 offsetuint8 datatypeuint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

修改后完整代码

从“LaserScan”和“PointCloud2”参数对比可以看出，“PointCloud2”可以满足三维显示的需求，因此修改rplidar_ros。其中“client.cpp”不需要修改，“node.cpp”修改如下：

/**  RPLIDAR ROS NODE**  Copyright (c) 2009 - 2014 RoboPeak Team*  http://www.robopeak.com*  Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd.*  http://www.slamtec.com**/
/** Redistribution and use in source and binary forms, with or without* modification, are permitted provided that the following conditions are met:** 1. Redistributions of source code must retain the above copyright notice,*    this list of conditions and the following disclaimer.** 2. Redistributions in binary form must reproduce the above copyright notice,*    this list of conditions and the following disclaimer in the documentation*    and/or other materials provided with the distribution.** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.**/#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_srvs/Empty.h"
#include "rplidar.h"#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif#ifndef DEG2RAD
#define DEG2RAD(x) ((x)*M_PI/180.)
#endifusing namespace rp::standalone::rplidar;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
pcl::PointCloud<pcl::PointXYZ> cloud_msg;
RPlidarDriver * drv = NULL;void publish_scan(ros::Publisher *pub, ros::Publisher *cloud_pub, rplidar_response_measurement_node_t *nodes,size_t node_count, ros::Time start,double scan_time, bool inverted,float angle_min, float angle_max,std::string frame_id)
{static int scan_count = 0;sensor_msgs::LaserScan scan_msg;scan_msg.header.stamp = start;scan_msg.header.frame_id = frame_id;scan_count++;bool reversed = (angle_max > angle_min);if ( reversed ) {scan_msg.angle_min =  M_PI - angle_max;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 = 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].distance_q2/4.0f/1000;if (read_value == 0.0)scan_msg.ranges[i] = std::numeric_limits<float>::infinity();elsescan_msg.ranges[i] = read_value;scan_msg.intensities[i] = (float) (nodes[i].sync_quality >> 2);}} else {for (size_t i = 0; i < node_count; i++) {float read_value = (float)nodes[i].distance_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].sync_quality >> 2);}}cloud_msg.header.frame_id = "laser";cloud_msg.height = 1;int count = scan_msg.scan_time / scan_msg.time_increment;cloud_msg.width  = count;cloud_msg.points.resize(cloud_msg.width * cloud_msg.height);for(int i = 0; i < count; i++) {float degree = RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * i);if(scan_msg.ranges[i]){cloud_msg.points[i].x = scan_msg.ranges[i]*cos(DEG2RAD(degree));cloud_msg.points[i].y = scan_msg.ranges[i]*sin(DEG2RAD(degree));cloud_msg.points[i].z = 0;}else{cloud_msg.points[i].x = 6*cos(DEG2RAD(degree));cloud_msg.points[i].y = 6*sin(DEG2RAD(degree));cloud_msg.points[i].z = 0;}ROS_INFO(": [%f, %f]", degree, scan_msg.ranges[i]);}pub->publish(scan_msg);pcl_conversions::toPCL(ros::Time::now(), cloud_msg.header.stamp);cloud_pub->publish(cloud_msg);
}bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
{u_result     op_result;rplidar_response_device_info_t devinfo;op_result = drv->getDeviceInfo(devinfo);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;
}bool checkRPLIDARHealth(RPlidarDriver * drv)
{u_result     op_result;rplidar_response_device_health_t healthinfo;op_result = drv->getHealth(healthinfo);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;}
}bool stop_motor(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
{if(!drv)return false;ROS_DEBUG("Stop motor");drv->stop();drv->stopMotor();return true;
}bool start_motor(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
{if(!drv)return false;ROS_DEBUG("Start motor");drv->startMotor();drv->startScan();;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::Publisher cloud_pub = nh.advertise<PointCloud> ("point_cloud2", 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);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))) {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();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];size_t   count = _countof(nodes);start_scan_time = ros::Time::now();op_result = drv->grabScanData(nodes, count);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);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, &cloud_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);start_node = i-1;i = count -1;while (nodes[i--].distance_q2 == 0);end_node = i+1;angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);publish_scan(&scan_pub, &cloud_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, &cloud_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;
}

代码修改详解

在“node.cpp”中主要做了以下几点修改：
1.添加“pointcloud2”的头文件

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>

2.修改宏定义

#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif#ifndef DEG2RAD
#define DEG2RAD(x) ((x)*M_PI/180.)
#endif

3.声明<PointCloud>和定义需要发布的消息cloud_msg

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
pcl::PointCloud<pcl::PointXYZ> cloud_msg;

3.定义消息发布结点

ros::Publisher cloud_pub = nh.advertise<PointCloud> ("point_cloud2", 1000);

4.修改publish_scan函数

void publish_scan(ros::Publisher *pub,rplidar_response_measurement_node_t *nodes,size_t node_count, ros::Time start,double scan_time, bool inverted,float angle_min, float angle_max,
std::string frame_id)

在函数参数传递时加入ros::Publisher *cloud_pub

void publish_scan(ros::Publisher *pub, ros::Publisher *cloud_pub, rplidar_response_measurement_node_t *nodes,size_t node_count, ros::Time start,double scan_time, bool inverted,float angle_min, float angle_max,std::string frame_id)

5.publish_scan函数中添加PointCloud2参数的赋值，并在向LaserScan发布消息的同时向PointCloud2发布消息

cloud_msg.header.frame_id = "laser";cloud_msg.height = 1;int count = scan_msg.scan_time / scan_msg.time_increment;cloud_msg.width  = count;cloud_msg.points.resize(cloud_msg.width * cloud_msg.height);for(int i = 0; i < count; i++) {float degree = RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * i);if(scan_msg.ranges[i]){cloud_msg.points[i].x = scan_msg.ranges[i]*cos(DEG2RAD(degree));cloud_msg.points[i].y = scan_msg.ranges[i]*sin(DEG2RAD(degree));cloud_msg.points[i].z = 0;}else{cloud_msg.points[i].x = 6*cos(DEG2RAD(degree));cloud_msg.points[i].y = 6*sin(DEG2RAD(degree));cloud_msg.points[i].z = 0;}ROS_INFO(": [%f, %f]", degree, scan_msg.ranges[i]);}pub->publish(scan_msg);pcl_conversions::toPCL(ros::Time::now(), cloud_msg.header.stamp);cloud_pub->publish(cloud_msg);

测试

1.运行代码

roslaunch rplidar_ros view_rplidar.launch

2.rviz界面设置
2.1.添加PointCloud2模块
点击右下角“Add”按钮

选择“PointCloud2”并修改显示的名称，“OK”保存

2.2.修改参数
“Topic”一定要和代码中的“Topic”名称相同，要不会出错（“States”会显示“error”）
修改“Size”参数，调整点云显示大小

“Size”设置太小会被“LaserScan”显示的红色点遮盖，在此设置为0.05，可以看到红色点与白色点重合。
2.3.保存设置
设置完后记得保存，这样下次再运行的时候就不用再添加、配置“PointCloud2”，当然这要覆盖原有的“rplidar.rviz”文件，所以大家自己斟酌咯。

随后再运行一下语句的时候就会自动调用之前配置好的文件

roslaunch rplidar_ros view_rplidar.launch

三维显示

如果想生成三维的效果，只用再函数publish_scan中添加PointCloud2参数中的z轴坐标即可，上述代码中z坐标为0。

cloud_msg.points[i].z = 0;

ROS——rplidar在rviz中三维显示相关推荐

ROS开发之如何将RPLidar数据在RViz中三维显示？
文章目录 0.引言 1.安装pcl和eigen 2.修改node.cpp 3.执行代码 4.三维显示 0.引言笔者研究的课题关于RPLidar二维激光雷达设备研发,需要将二维激光雷达数据结合其他 ...
ROS在rviz中实时显示轨迹和点
记录一个简单的例子方便复制: 通过使用 nav_msgs::Path geometry_msgs::PoseStamped geometry_msgs::PointStamped 来实现rviz中可视 ...
[ROS学习笔记1]在RViz中查看激光雷达数据
目录说在前面我的软件搭配激光雷达介绍实现方法创建工作空间编写源文件添加环境变量连接雷达端口运行rviz 结果说在前面情况介绍:本人今年大四在读,老师给了"轮式机器人定位 ...
通过RViz中的InteractiveMarkers在ROS中仿真力和力矩(wrench.force和wrench.torque)
对于机器人的视觉这些信息,很容易在ROS中呈现,但是对于ROS中力和力矩信息的展示,通常在实验室的时候使用真实的机器人和传感器,这个问题不需要考虑,直接读取真实的力和力矩传感器,然后通过ROS消息发布 ...
ROS（Noetic）学习笔记创建机器人URDF模型并在rviz中显示过程中遇到的一些问题
背景环境: Ubuntu 20.04 ROS-Noetic 所学教程:<ROS机器人开发实践>chapter 6_2 问题描述及解决方法: 报错:无法启动节点<robot_state ...
【ROS问题】在Rviz中显示带有纹理信息的模型文件
问题描述利用sw_urdf_exporter插件可以方便快捷的导出urdf package,省去了用户自己编写urdf文件的过程.但是,由于该插件导出的link是.STL格式,因此每个连杆都不带纹理 ...
ROS使用禾赛PandarXT-16雷达在RVIZ中进行仿真
相关环境 win10 ubuntu18.04 ros-melodic 安装相关驱动雷达的git网站,安装对应依赖库 sudo apt install libpcap-dev libyaml-cpp- ...
ROS学习之tf在rviz中的显示
昨天粗略看了一下ros中rviz的用法.事实,他就是一个可视化的工具.wiki的用户手册:http://wiki.ros.org/rviz/UserGuide 今天在学习tf的教程时,首先是演示了一个 ...
Rviz中如何导入自定义障碍物 | 从建模到导出urdf到导入rviz | Ros中如何导入障碍物 | sw2urdf的下载和使用 | MeshLa的下载和使用
配置: Ubuntu20.04 Solidworks2016 sw2urdf1.5.1(GitHub下载链接) MeshLab(GitHub下载链接) 目标本文希望在Solidworks中自己完成障 ...

ROS——rplidar在rviz中三维显示