需求分析
rplidar A1和rplidar A2使用相同的接口和SDK,因此可以均可以使用该代码。官方的DEMO实现了激光雷达的二维扫描,并将扫描结果事实显示在rviz中,可以满足基本的需求。官方DEMO中将扫描结果在rviz的“LaserScan”中显示,但是由于“LaserScan”只能显示二维坐标,所以当需要使用二维激光雷达三维建模时官方DEMO便不能使用,因此我将其移植到rviz的“PointCloud2”中显示。
消息类型查看
输入以下命令查询“LaserScan”参数:
rosmsg show sensor_msgs/LaserScan
显示如下
std_msgs/Header header
uint32 seq
time stamp
string 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 header
uint32 seq
time stamp
string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
uint8 INT8=1
uint8 UINT8=2
uint8 INT16=3
uint8 UINT16=4
uint8 INT32=5
uint8 UINT32=6
uint8 FLOAT32=7
uint8 FLOAT64=8
string name
uint32 offset
uint8 datatype
uint32 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.)
#endif
using 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();
else
scan_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();
else
scan_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 instance
drv = 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 info
if (!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 node
while (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 them
float 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;