基于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
", 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
", 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="pyrokinecist@aliyun.com">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