ROS中發布激光掃描消息

本文轉載自查看原文 2017-11-15 21:54 5607 ROS/ 傳感器

　　激光雷達工作時會先在當前位置發出激光並接收反射光束，解析得到距離信息，而后激光發射器會轉過一個角度分辨率對應的角度再次重復這個過程。限於物理及機械方面的限制，激光雷達通常會有一部分“盲區”。使用激光雷達返回的數據通常可以描繪出一幅極坐標圖，極點位於雷達掃描中心，0-360°整周圓由掃描區域及盲區組成。在掃描區域中激光雷達在每個角度分辨率對應位置解析出的距離值會被依次連接起來，這樣，通過極坐標表示就能非常直觀地看到周圍物體的輪廓，激光雷達掃描范圍示意圖可以參見下圖。

　　激光雷達通常有四個性能衡量指標：測距分辨率、掃描頻率（有時也用掃描周期）、角度分辨率及可視范圍。測距分辨率衡量在一個給定的距離下測距的精確程度，通常與距離真實值相差在5-20mm；掃描頻率衡量激光雷達完成一次完整掃描的快慢，通常在10Hz及以上；角度分辨率直接決定激光雷達一次完整掃描能返回多少個樣本點；可視范圍指激光雷達完整掃描的廣角，可視范圍之外即為盲區。

　　目前，移動機器人的研究中已經大量使用激光雷達輔助機器人的避障導航。通常激光雷達會提供ROS驅動，如果沒有的話我們也可以自己采集激光數據后按照ROS中定義的消息格式將信息發布出去。首先，輸入下面的指令查看LaserScan消息結構：

rosmsg show sensor_msgs/LaserScan

　　LaserScan消息結構如下：

std_msgs/Header header 　　uint32 seq 　　time stamp 　　string frame_id 　　 # in frame frame_id, angles are measured around the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis 
 float32 angle_min # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]
 float32 time_increment # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]
 float32 range_min # minimum range value [m]
float32 range_max        # maximum range value [m]
 float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units]. If your device does not provide intensities, please leave the array empty.

　　以下圖為例，該激光雷達掃描范圍為270°，角度分辨率為0.25°，掃描距離為0~20m，每掃描一圈會得到1081個點：

　　那么該激光雷達發布的LaserScan消息內容如下:

angle_min= -135 * (pi/180);       //angle correspond to FIRST beam in scan ( in rad)
angle_max= 135 * (pi/180);        //angle correspond to LAST beam in scan ( in rad)
angle_increment =0.25 * (pi/180); // Angular resolution i.e angle between 2 beams

// lets assume sensor gives 50 scans per second. i.e every 20 milli seconds 1 scan with 1081 beams.
// Each beam is measured in  (20 ms/ 1081 ) ~ = 0.0185 ms
time_increment  = (1 / 50) / (1081); 

scan_time = ;    // scan is collected at which time
range_min =0 ;   // in meters
range_max = 20;  // scan can measure upto this range

// ranges is array of 1081 floats for each laser beam
ranges[0] =      //distance measure corresponds to angle -135 deg
ranges[1] =      //distance measure corresponds to angle -134.75 deg
    .
    .
    .
ranges[1080] =   //distance measure corresponds to angle +135 deg

// To understand Intensities 
// if a laser beam hits reflective surface like glass it will have intensity 1. 
// And if beam hit some surface which absorbs laser , then intensity is zero. 
// Middle values are different surfaces in between.

　　下面的代碼模擬了激光雷達的數據，並將sensor_msgs/LaserScan消息發布到/scan話題上：

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "laser_scan_publisher");

    ros::NodeHandle n;

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

    unsigned int num_readings = 100;
    double laser_frequency = 40;
    double ranges[num_readings];
    double intensities[num_readings];

    int count = 0;

    ros::Rate r(1.0);

    while(n.ok())
    {
        //generate some fake data for our laser scan
        for(unsigned int i = 0; i < num_readings; ++i)
        {
            ranges[i] = count;
            intensities[i] = 100 + count;
        }
        ros::Time scan_time = ros::Time::now();

        //populate the LaserScan message
        sensor_msgs::LaserScan scan;
        scan.header.stamp = scan_time;
        scan.header.frame_id = "base_link";
        scan.angle_min = -1.57;
        scan.angle_max = 1.57;
        scan.angle_increment = 3.14 / num_readings;
        scan.time_increment = (1 / laser_frequency) / (num_readings);
        scan.range_min = 0.0;
        scan.range_max = 100.0;
        scan.ranges.resize(num_readings);
        scan.intensities.resize(num_readings);
        for(unsigned int i = 0; i < num_readings; ++i)
        {
            scan.ranges[i] = ranges[i];
            scan.intensities[i] = intensities[i];
        }

        scan_pub.publish(scan);

        ++count;

        r.sleep();
    }
}

　　可以在rviz中將激光數據點顯示出來：Fixed Frame修改為base_link，添加LaserScan並將Topic設為/scan

　　如果Fixed Frame為map，為了能正確顯示出激光掃描點來，需要發布map和base_link之間的坐標變換關系（因為我們的激光數據是相對於base_link坐標系描述的）。如果這兩個參考系不發生相對位置變化，那么可以用static_transform_publisher工具發布兩個參考系之間的靜態坐標變換。

　　命令的格式如下：

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms

　　以上兩種命令格式，需要設置坐標的偏移和旋轉參數，偏移參數都使用相對於x、y、z三軸的坐標位移。旋轉參數第一種命令格式使用以弧度為單位的 yaw/pitch/roll三個角度，第二種命令格式使用四元數表達旋轉角度。發布頻率以ms為單位，一般100ms比較合適。

　　static_transform_publisher is designed both as a command-line tool for manual use, as well as for use within roslaunch files for setting static transforms. For example:

<launch>
　　<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>

ROS中使用激光雷達(鐳神智能)

　　LS01C是深圳市鐳神智能系統有限公司研發的激光三角測距系統。在機械旋轉模塊的帶動下，LS01C 的高頻核心測距模塊將進行順時鍾旋轉，從而實現對周圍環境進行360°掃描測距。LS01C通過uart 串口信號與外部系統通訊，默認每秒采樣3600點、掃描頻率10hz，最大掃描距離6m，角度分辨率為1度。

　　將LS01C的ROS驅動文件（官網上沒有下載連接，直接打電話給客服要的）解壓重命名為talker后復制到catkin_ws/src下面，然后使用catkin_make進行編譯

　　插入USB后在終端中輸入以下命令查看USB轉串口設備：

　　在發現ttyUSB0后在終端中輸入下面命令給USB 轉串口設置權限：

sudo chmod 666 /dev/ttyUSB0

　　修改launch文件中的串口名，改為我們插入的ttyUSB0

<launch>
    <node name="talker" pkg="talker" type="talker">
　　        <param name="scan_topic" value="scan"/>           <!-- 設置激光數據topic名稱 -->
　　        <param name="laser_link" value="laser_link"/>     <!-- 激光坐標 -->
　　        <param name="serial_port" value="/dev/ttyUSB0"/>  <!-- 雷達連接的串口 -->
    </node>
</launch>