RPLIDAR A2 rviz顯示雷達資料教程

蘇守坤發表於2018-07-03

扣扣技術交流群:460189483

1. 簡介

  RPLIDAR A2的執行教程在這裡不做重點講解,官方教程已經介紹的很詳細,在這裡跟大家解釋下例程中是怎麼在RVIZ中事實顯示影像的。

  執行下面這句話時,可以看到rviz自動開啟,並且在影像中顯示出了鐳射雷達掃描到的資料資訊,那麼鐳射雷達的資料是如何傳遞到rviz中的那?

roslaunch rplidar_ros view_rplidar.launch

2. view_rplidar.launch分析

開啟view_rplidar.launch

<launch>
  <include file="$(find rplidar_ros)/launch/rplidar.launch" />

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
</launch>

我們發現在其中呼叫了rplidar.launch,使用include命令是為了減少冗餘程式碼的數量,簡化程式碼。rplidar.launch如下

<launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="115200"/>
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node>
</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功能模組定義的標準資訊格式



我們可以在命令列中輸入下列命令來檢視LaserScan的資料格式

rosmsg show sensor_msgs/LaserScan
  • 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




在這個圖中可以清楚的看到程式只發布了一個topic——”/scan”,同時在我們的rviz中我們也可以看到在”topic”選項後面對應的是”/scan”,即我們的分析完全正確。


相關文章