ROS——rplidar A1在rviz中实时显示分析

短命女 2022-06-06 12:41 187阅读 0赞

# 1. 简介 #

RPLIDAR A1的运行教程在这里不做重点讲解，[官方教程][Link 1]已经介绍的很详细，在这里跟大家解释下例程中是怎么在RVIZ中事实显示图像的。

运行下面这句话时，可以看到rviz自动打开，并且在图像中显示出了激光雷达扫描到的数据信息,那么激光雷达的数据是如何传递到rviz中的那？

roslaunch rplidar_ros view_rplidar.launch

# 2. view\_rplidar.launch分析 #

打开`view_rplidar.launch`

我们发现在其中调用了`rplidar.launch`，使用`include`命令是为了减少冗余代码的数量，简化代码。`rplidar.launch`如下

在这个launch文件中可以清楚的看到，在其中定义了串口的名字`"/dev/ttyUSB0"`，波特率`"115200"`等等一系列的串口配置信息，正是由于这些配置信息，才可以在`rplidarNode`节点中获取激光雷达发送过来的数据信息。

# 3.分析如何发送数据到rviz #

看到这里可能很多朋友会比较疑惑是如何发送数据到rviz平台的，在此我们打开`rplidar_ros/src/node.cpp`文件,在193行左右可以发现这个消息发布者的节点名字和类型，类型和我们以往使用的不太一样。

ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);

没错，这个`<sensor_msgs::LaserScan>`类型就是rviz中`LaserScan`功能模块定义的标准信息格式

![SouthEast][]  
  
我们可以在命令行中输入下列命令来查看LaserScan的数据格式

rosmsg show sensor_msgs/LaserScan

运行结果如图所示：

![SouthEast 1][]  
  
这个时候我们再来对比 `rplidar_ros/src/node.cpp`文件中的 `void 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)
    {
        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);
            }
        }
    
        pub->publish(scan_msg);
    }

在函数中可以清楚的看到有对下列变量进行赋值操作

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

因此可以断定正是通过对这些数据赋值，然后再讲消息发布出去，rviz中的LaserScan便会自动订阅这些消息，从而将激光雷达中的数据显示出来，如果还是有疑问的话大家可以在运行`roslaunch rplidar_ros view_rplidar.launch`的同时，在行的命令行中输入下列命令查看ROS话题发发布订阅情况：

rosrun rqt_graph rqt_graph

![SouthEast 2][]  
  
在这个图中可以清楚的看到程序只发布了一个topic——”/scan”，同时在我们的rviz中我们也可以看到在”topic”选项后面对应的是”/scan”，即我们的分析完全正确。  
  
![SouthEast 3][]

[Link 1]: http://wiki.ros.org/rplidar
[SouthEast]: /images/20220606/5855898bb53944ba9ca436390715ca05.png
[SouthEast 1]: /images/20220606/40ea79f1b4bd45c19d44ecc88cc5b675.png
[SouthEast 2]: /images/20220606/d58cca7cd07b440b9426f84f7d895ad6.png
[SouthEast 3]: /images/20220606/3120aaa19917498282cd189233004462.png