3D激光雷达价格不菲，在研究过程中，可以尝试将2D激光雷达的点云输出改为3D的点云输出，以进行3D激光雷达的部分试验。本文将安装Rplidar的依赖项并且在ROS的Rviz上可视化。并将Rplidar的/scan信息改为/PointCloud2信息进行输出，以通过2D激光雷达实现3D激光雷达的模拟应用。
建立工作空间（也可以利用现有的），编译包
$ mkdir -p ~/turtlebot_ws/src
$ cd ~/turtlebot_ws/src
$ git clone https://github.com/ncnynl/rplidar_ros.git#这部分要到思岚科技的github去找具体链接，到目前克隆链接是https://github.com/robopeak/rplidar_ros.git
添加环境变量
source /home/ubu/turtlebot_ws/devel/setup.bash
source ~/.bashrc
检查、设置端口权限
ls -l /dev |grep ttyUSB
sudo chmod 666 /dev/ttyUSB0
在此Rplidar应该已经可以使用
在工作空间下运行rplidar和打开rviz查看
roslaunch rplidar_ros rplidar.launch
rosrun rviz rviz
设置相关配置后可以看见2D雷达的输出图像
从“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,
float max_distance,
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 = 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].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 = 16;
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 j = 0; j < cloud_msg.height; j++) {
for(int i = count*j; i < count+count*j; i++) {
float degree = RAD2DEG(scan_msg.angle_min + scan_msg.angle_increment * (i-count*j));
float k=j;
if(scan_msg.ranges[(i-count*j)])
{
cloud_msg.points[i].x = scan_msg.ranges[(i-count*j)]*cos(DEG2RAD(degree));
cloud_msg.points[i].y = scan_msg.ranges[(i-count*j)]*sin(DEG2RAD(degree));
cloud_msg.points[i].z = k/10-0.8;
}
else
{
cloud_msg.points[i].x = 6*cos(DEG2RAD(degree));
cloud_msg.points[i].y = 6*sin(DEG2RAD(degree));
cloud_msg.points[i].z = k/10-0.8;
}
ROS_INFO(": [%f, %f]", degree, scan_msg.ranges[(i-count*j)]);
}
}
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) {
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;
}
bool checkRPLIDARHealth(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_health_t healthinfo;
op_result = drv->getHealth(healthinfo);
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;
}
}
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(0,1);
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;
float max_distance = 8.0;
int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree
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/*256000*/);//ros run for A1 A2, change to 256000 if A3
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);
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);
if (!drv) {
ROS_ERROR("Create Driver fail, exit");
return -2;
}
// make connection...
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
ROS_ERROR("Error, cannot bind to the specified serial port %s.",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();
RplidarScanMode current_scan_mode;
op_result = drv->startScan(0,1,0, ¤t_scan_mode);
if(op_result != RESULT_INVALID_DATA)
{
//default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us
angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0);
max_distance = current_scan_mode.max_distance;
ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK ", current_scan_mode.scan_mode,
current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample));
}
else
{
ROS_ERROR("Can not start scan: RESULT_INVALID_DATA!");
}
ros::Time start_scan_time;
ros::Time end_scan_time;
double scan_duration;
while (ros::ok()) {
rplidar_response_measurement_node_t nodes[360*8];
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();
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_multiple = 1;
const int angle_compensate_nodes_count = 360*angle_compensate_multiple;
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, max_distance,
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, max_distance,
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, max_distance,
frame_id);
}
}
ros::spinOnce();
}
// done!
drv->stop();
drv->stopMotor();
RPlidarDriver::DisposeDriver(drv);
return 0;
}
1.运行代码
roslaunch rplidar_ros view_rplidar.launch
rosrun rviz rviz
配置后可见
