ROS——rplidar在rviz中三维显示

ゝ一纸荒年。 2022-06-03 22:27 154阅读 0赞

# 需求分析 #

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][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”按钮

![SouthEast][]  
  
选择“PointCloud2”并修改显示的名称，“OK”保存  
  
![SouthEast 1][]

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

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

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

roslaunch rplidar_ros view_rplidar.launch

# 三维显示 #

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

cloud_msg.points[i].z = 0;

[DEMO]: http://wiki.ros.org/rplidar
[rplidar_ros]: https://github.com/robopeak/rplidar_ros
[SouthEast]: /images/20220604/4e3dc05a8c0149d1af7a0f44e82ae8ed.png
[SouthEast 1]: /images/20220604/9a029ed5f3b847e18923586335075cd4.png
[SouthEast 2]: /images/20220604/7b2228df80c1403588a1d032c067dc53.png
[SouthEast 3]: /images/20220604/d1035080b1b442e7961048f7cca6157f.png