Rplidar學習（四）—— ROS下進行rplidar雷達數據採集源碼分析

1、子函數分析

一、發佈數據子函數

（1）雷達數據數據類型

Header header            # timestamp in the header is the acquisition time of 
                         # the first ray in the scan.
                         #
                         # 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.

 

 

（2）雷達發佈子函數分析

/***********************打印函數*************************/
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)                                //id
{
    static int scan_count = 0;
    sensor_msgs::LaserScan scan_msg;         //ros已有的雷達數據模型

    scan_msg.header.stamp = start;           //掃描起始時間
    scan_msg.header.frame_id = frame_id;     //序列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);
}

 

 

二、獲得設備信息子函數

/*****************獲得設備信息**************************/
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;
}

 

 

三、檢查rplidar設備健康信息

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;
}

 

 

五、main函數分析

int main(int argc, char * argv[])
{
    ros::init(argc, argv, "rplidar_node");

    std::string serial_port;          //串口號
    int serial_baudrate = 115200;     //串口波特率
    std::string frame_id;             //id
    bool inverted = false;            //是否轉換標誌位
    bool angle_compensate = true;     //角度補償標誌位

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

    //launch能夠進行一些初始化
    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)  //建立失敗退出-2
    {
        fprintf(stderr, "Create Driver fail, exit\n");
        return -2;
    }

    // make connection...創建連接，失敗返回-1
    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;

                    //初始化，清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)  //距離不是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,
                             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, &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, 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;
}

 

 

2、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文件

（1）格式：

roslaunch package_name launch_file_name

 

 Tip1: rosrun只能運行一個nodes， roslaunch能夠同時運行多個nodes

（2）詳細顯示(request verbosity)

（3） 結束launch文件

 

 

ctrl+c

 

 

 

 

二、建立launch文件

(1) launch文件通常以.launch後綴做爲文件名，放在package的launch文件夾下。最簡單的launch文件能夠僅包含幾個nodes。

 

(2) Launch文件是XML文件，每一個XML文件必須有一個root element。而launch文件的root element由一對launch 標籤訂義。

<launch>

...

</launch>

 

 

(3) launch文件的核心是一系列node elements，每一個node element啓動一個node。Node element看起來以下

 

<node

  pkg=」package_name」 type=」executable_name」 name=」node_name」

/>

#Tip1: 最後的「/」是必不可少的。

#Tip2: 也能夠寫成<node pkg=」..」 type=」...」 name=」...」></node>

#若是該node中有其餘tags，則必須使用這種形式。

 

 

(4) 一個node element包含三個必須的屬性：pkg, type, name.

　　pkg和type屬性指出ROS應該運行哪一個pkg中的哪一個node，注意：此處的type是可執行文件的名稱，而name則是能夠任意給出的，它覆蓋了原有文件中ros::init指定的node name。

 

(5) node日誌文件(log file)

　　運行roslaunch和rosrun運行單個節點的區別之一是，默認狀況下，roslaunch運行的nodes的標準輸出會重定向到log file，而不是控制檯

 

~/.ros/log/run_id/node_name-number-stdout.log

 

 

(6) 輸出到控制檯

　　用output屬性， output=」screen」；這種方法僅顯示一個node。

　　若顯示全部nodes的輸出，用--screen命令行。

roslaunch --screen package_name launch_file_name

　　若是正在運行的文件沒有顯示想要對輸出，能夠查看該node屬性集中是否有 output=」screen」.

 

 

 

(7) 在獨立的窗口運行各nodes

 

　　咱們在各自的termin運行rosrun node_name；可是運行roslaunch時，全部的nodes共用一個相同的terminal，這對於那些須要從控制檯輸入的nodes很不方便。可使用launch-prefix屬性。

 

launch-prefix=」command-prefix」

Eg：launch-prefix=」xterm -e」    等價於  xterm -e rosrun turtlesim turtle_teleop_key

 

　　xterm 命令表示新建一個terminal； -e參數告訴xterm執行剩下的命令行。

 

　　固然，launch-prefix屬性不單單限於xterm。它可用於調試（經過gdb或valgrind），或用於下降進程的執行順序（經過nice）.