基於Ubuntu1604+ROS-kinetic+roscpp的激光雷達定位算法從零開始移植

調試的過程太麻煩了，因此打算詳細解釋一下每步的含義，很多地方懂了之后發現其實很簡單，但是學起來卻發現很多地方無從下手，因為資料太少了，真的都是不斷踩坑一點一點摸索出來的，寫以此文以便后人乘涼

此處將展示一個完全獨立的節點的編寫過程，如果讀者打算移植算法到ROS平台可以稍作閱讀，首先是在仿真環境下要產生可以訂閱的激光雷達數據和地圖數據，最開始嘗試了fake系列包的定位，但是嘗試了幾天之后發現，fake定位是真的fake，使用的是fake_localization，沒有輸出任何可用的傳感器數據，完全是使用的是里程計數據Odometry轉化為tf數據，因此仿真時機器人會瘋狂撞牆，因此需要換一個仿真

然后找到了gmapping的仿真，使用的是古月居的Package，鏈接文末給出了，不要下載rbx的，那個是面向ubuntu1404-ros-indigo的包，每個ubuntu版本有對應的ros包，不能互通，使用前建議先查看一下環境

這個是用gazebo仿真一個帶障礙物和機器人的環境
roslaunch mrobot_gazebo mrobot_laser_nav_gazebo.launch
這個是用啟動gmapping仿真（應該是arbotix）
roslaunch mrobot_navigation gmapping_demo.launch
這個是輸出cmd_vel的控制台，用於控制小車移動的
roslaunch mrobot_teleop mrobot_teleop.launch

前兩個仿真的界面可以關閉，只要不關控制台，話題是一直存在的，調試程序階段可以關掉，以節省CPU算力，后面不建議關閉，因為需要查看小車實時位置

文件存儲格式參考其他的數據包，也可以在文末的Git鏈接里面下載到源碼，編譯IDE使用的是roboware-studio，一個基於vscode的編輯器，有不少ROS的特性

然后自己建立新的結點訂閱數據，數據類型使用“rostopic info /xxx”來查看類型，也可以在rviz下點add查看發布的數據類型，設置訂閱隊列長度為1，這樣只訂閱最新的消息

int main(int argc, char **argv) {
    //初始化節點
    ros::init(argc, argv, "laser_listener");
    ros::NodeHandle nh;

    //訂閱gmapping在gazebo仿真下的數據
    ros::Subscriber subMapParam = nh.subscribe("/map_metadata", 1, mapParamCallback);
    ros::Subscriber subMap = nh.subscribe("/map", 1, mapCallback);
    ros::Subscriber subScan = nh.subscribe("/scan", 1, laserCallback);

    //循環讀取
    ros::spin();
    return 0;
}

然后獲取地圖參數，原數據名字為“MapMetaData”，但筆者選擇了之前的命名方式mapParam，然后讀取數據，這里只能用 “->” 來讀取，不能使用 “.” 來獲取子元素，可以使用ROS_INFO來輸出數據到控制台

void mapParamCallback(const nav_msgs::MapMetaData::ConstPtr &msg) {
    mapParam.oriMapCol = msg->width;
    mapParam.oriMapRow = msg->height;
    mapParam.mapResol = msg->resolution;
    mapParam.mapOriX = msg->origin.position.x;
    mapParam.mapOriY = msg->origin.position.y;
    //ROS_INFO("%d %d %lf %lf %lf\n", oriMapRow, oriMapCol, mapOriX, mapOriY, mapResol);
}

然后獲取地圖Map數據，這里用了兩個保險，分別是檢測是否有數據輸入，沒有則等待輸入，另一個是是否處理完Map數據，如果沒有處理完則特生匹配會報錯，數據儲存在msg下的data里面，雖然能自動補全出其他子元素，但是很多並不能查看，具體格式可以參考文末官方文檔，讀取方式用數組的方式讀取，沒有使用c++的迭代器，然后就是數據格式，因為ROS使用的是柵格地圖，一種基於概率的地圖表示方法，因此用0表示空閑（探測過但是沒有障礙物），100表示占據（探測過並確認有障礙物），用1~99表示存在障礙物的概率（探測過），用255（在unsigned int 8下也是-1）表示未知（未探測過），這和筆者前面寫的算法的表示方法不同，筆者的表示方式中0是未知，1是占據（有障礙物），-1（255）是空閑（無障礙物），所以做了一個映射，（暫時沒有考慮概率方式的計算），然后是生成貝葉斯概率距離地圖和用LSD算法提取直線信息

void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg) {
    if (mapParam.oriMapCol <= 0 || mapParam.oriMapRow <= 0)
        return;
    isMapReady = false;
    mapValue = cv::Mat::zeros(mapParam.oriMapRow, mapParam.oriMapCol, CV_8UC1);
    //int max = 0;
    int cnt_col, cnt_row;
    //ROS_INFO("size%d %d %lf\n", msg->info.height, msg->info.width, msg->info.resolution);

    //取消注釋FILE相關的可以輸出地圖到文件，在Windows下運行main_on_windows使用
    //FILE *fp = fopen("mapValue.txt", "w+");
    for (cnt_row = 0; cnt_row < mapParam.oriMapRow; cnt_row++) {
        for (cnt_col = 0; cnt_col < mapParam.oriMapCol; cnt_col++) {
            uint8_t value = msg->data[cnt_row * mapParam.oriMapCol + cnt_col];
            if (value == 255) {
                //fprintf(fp, "%d ", 0);
                mapValue.ptr<uint8_t>(cnt_row)[cnt_col] = 0;
            }
            else if (value == 0) {
                //fprintf(fp, "%d ", 255);
                mapValue.ptr<uint8_t>(cnt_row)[cnt_col] = 255;
            }
            else {
                //fprintf(fp, "%d ", 1);
                mapValue.ptr<uint8_t>(cnt_row)[cnt_col] = 1;
            }
        }
    }
    //fclose(fp);
    // cv::imshow("mapValue", mapValue);
    // cv::waitKey(1);
    //計算mapCache，用於特征匹配的先驗概率
    double z_occ_max_dis = 2;
    mapCache = mylsd::createMapCache(mapValue, mapParam.mapResol, z_occ_max_dis);
    //LineSegmentDetector 提取地圖邊界直線信息
    LSD = mylsd::myLineSegmentDetector(mapValue, mapParam.oriMapCol, mapParam.oriMapRow, 0.3, 0.6, 22.5, 0.7, 1024);
    isMapReady = true;
}

最后是持續輸入的激光雷達數據，因為地圖數據只有更新才會傳入，因此必須等待地圖數據發布並且處理完才能處理激光雷達數據並做特征匹配，所以加了一個保險，這里儲存的方式是距離和增量角度，因此做一個簡單的轉換，格式是float，如果用double類型會報錯，然后做特征匹配並用OpenCV輸出（目前情況在每次地圖更新之后會清空坐標點，可以自行增加緩存機制來儲存位置信息），當然，讀者也可以發布tf坐標然后用rviz顯示

void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
    //等待地圖處理結束
    if (isMapReady == false)
        return;
    //讀取激光雷達數據
    std::vector<float> ranges = msg->ranges;
    int len_lp = 0;
    for (int i = 0; i < ranges.size(); i++) {
        if (ranges[i] != INFINITY) {
            lidarPointPolar[i].range = ranges[i];
            //角度是用增量式方式來儲存的
            lidarPointPolar[i].angle = msg->angle_min + i * msg->angle_increment;
            lidarPointPolar[i].split = false;
            len_lp++;
        }
        //ROS_INFO("No.%d RPLIDAR:%lf %lf", i, lidarPointPolar[i].range, lidarPointPolar[i].angle);
    }

    if (len_lp != 0) {
        //匹配雷達特征到地圖特征 返回像素坐標和真實坐標
        myrdp::structFeatureScan FS = FeatureScan(mapParam, lidarPointPolar, len_lp, 3, 0.04, 0.5);
        imshow("RPLidar", FS.lineIm);
        waitKey(1);

        double estimatePose_realworld[3];
        double estimatePose[3];
        Mat poseAll;
        //數據接口轉換
        structFA FA = trans2FA(FS, LSD, mapParam, lidarPointPolar, len_lp);
        //特征匹配
        myfa::FeatureAssociation(FS.lineIm, FA.scanLinesInfo, FA.mapLinesInfo, mapParam, FA.lidarPos, LSD.lineIm, \
            mapCache, mapValue, FA.ScanRanges, FA.ScanAngles, estimatePose_realworld, estimatePose, poseAll);

        //將圖像坐標加入地圖中
        LSD.lineIm.ptr<uint8_t>((int)estimatePose[1])[(int)estimatePose[0]] = 255;
        imshow("lineIm", LSD.lineIm);
        waitKey(1);
    }
}

目前數據僅僅是做了最基本的特征匹配，還沒有增加優化算法，因此在特征不明顯的地方漂移比較大，但是在特征點明顯的地方，比如拐角處定位效果還是不錯的

然后是編譯需要的CMakeLists，分別是生成文件夾的名字“project(lsd)”，c11特性，包含庫的路徑find_package及需要包含的數據類型（如topic類型），然后在catkin_package里面也添加對應的數據類型，然后在包含路徑里面添加catkin，因為筆者用到了opencv，所以添加了opencv路徑，之后是生成可執行文件和添加鏈接庫

cmake_minimum_required(VERSION 2.8.3)
project(lsd)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  geometry_msgs
)
find_package(OpenCV)

catkin_package(
  CATKIN_DEPENDS
    roscpp
    sensor_msgs
    geometry_msgs
  INCLUDE_DIRS include
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(lsd 
                  src/main_on_linux.cpp
                  src/baseFunc.cpp
                  src/FeatureAssociation.cpp
                  src/myLSD.cpp
                  src/myRDP.cpp)
target_link_libraries(lsd ${catkin_LIBRARIES})
target_link_libraries(lsd ${OpenCV_LIBS})

然后是package.xml，name里面是生成文件夾的名字，然后添加需要的數據類型（同上），需要同時添加build_depend和run_depend（可能部分庫可以只添加一項，為了不出錯，建議都添加）

<?xml version="1.0"?>
<package>
  <name>lsd</name>
  <version>0.0.0</version>
  <description>The test package</description>

  <maintainer email="[e-mail]">pyrokine</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>tf</build_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>tf</run_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
  <!-- Other tools can request additional information be placed here -->

  </export>
</package>

然后編譯完就可以執行了，理論上，此節點可以先於也可以晚於其他節點包括master節點開啟，如果運行時報錯提示特征匹配文件內出錯，目前只能重新開啟，正在修復該問題，如果編譯的時候提示一下錯誤，可以忽略 

 

 Github源碼：（在ROS文件夾里面）

https://github.com/Pyrokine/LineSegmentDetector17

感謝以下Geeks：

《ROS機器人開發實踐》源碼
https://github.com/huchunxu/ros_exploring
ros訂閱激光雷達/scan
https://blog.csdn.net/weixin_33910137/article/details/86873846
sensor_msgs/LaserScan Message
http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html
nav_msgs/OccupancyGrid Message
http://docs.ros.org/kinetic/api/nav_msgs/html/msg/OccupancyGrid.html