【源码学习笔记】Towards Efficient Trajectory Generation for Ground Robots beyond 2D Environment

前言

该文章是对2023年发表在IROS上的文献《Towards Efficient Trajectory Generation for Ground Robots beyond 2D Environment》源码的学习笔记，论文作者来自于浙江大学团队，代码地址为GitHub - ZJU-FAST-Lab/3D2M-planner。

一、整体概述

源码中共包括11个功能包，其中map_manager用于实现地图的初始化和计算，planner_algorithm用于实现前端的A*路径规划和后端的轨迹优化，planner_manager是整个程序的入口，定义了工程框架和算法的调度，read_pcd实现了pcd文件的读取和发布。

└─src
    ├─diablo-onboard-sdk-with-serial
    ├─fake_diablo
    ├─fake_ugv
    ├─globalmap_gene
    ├─map_manager
    │  ├─include
    │  ├─launch
    │  ├─param
    │  └─src
    ├─mpc
    ├─planner_algorithm
    │  ├─include
    │  └─src
    ├─planner_manager
    │  ├─include
    │  ├─launch
    │  ├─msg
    │  └─src
    ├─read_pcd
    │  ├─launch
    │  ├─PCDFiles
    │  └─src
    ├─rviz_plugins
    └─rviz_plugin_tutorials

二、程序流程

根据官方的使用教程【sh exp1.sh】，分别启动位于planner_manager的scene1.launch和位于read_pcd的read_exp1.launch，后者显然是读取地图pcd文件（暂时还没有细看），前者是整个程序的入口，首先初始化ros节点planner_manager_node，对地图（pcsmap_manager）、前端规划器（astar_search）和后端规划器（minco_traj_optimizer）初始化，然后实例化发布者和订阅者，最后通过ros::spin()函数处理回调函数队列。

程序流程

地图的初始化： 主要实现程序为PCSmap_manager.cpp和Gridmap.cpp，构建了occupancy_map、snowed_occupany_map、zcost_map、travelcost_map和surfel_map等地图，分别用于生成占用栅格地图、计算ESDF地图、计算地形粗糙度等，输入为全局点云地图，输出为可用于A*规划的栅格地图。

前端A*算法：主要实现程序为front_end_Astar.cpp，其初始化函数【astar_searcher.init()】是一个空函数，前端搜索实际上是在回调函数【targetRcvCallBack】中完成的，输入信息为起始终点状态和栅格化后的地图，输出为路径的节点集合。该部分算法采用的是经典的A*算法，可以视作封装好的函数接口。

后端轨迹优化：主要实现程序为back_end_optimizer.cpp，同样在回调函数【targetRcvCallBack】中完成，其输入为前端路径节点集合，输出为优化后的轨迹参数。该部分算法采用的优化目标比较复杂，与我的学习关系不大，略去。

在算法的主程序中定义了多个订阅者和发布者，但实际有效的订阅者只有：odom_sub订阅了话题“odom”，当有里程计信息发布时，调用回调函数【odomRcvCallBack】将状态标志位have_odom置为true（该标志位决定了是否进行路径规划），且保存里程计信息（在launch文件中odom被重映射为/ekf/ekf_odom）；target_sub订阅了话题“/goal”，该话题由rviz发布，当设定终点时，调用回调函数【targetRcvCallBack】，计算从起点到终点的cost、初始路径和优化后路径，该回调函数的输入为目标位姿，输出并发布初始及后端轨迹；target_sub2订阅了话题“/anchor_goal_3D”，当有话题信息发布时，调用回调函数【target2RcvCallBack】，不过在程序中该回调函数被设置为了空函数，没有意义；rcvmap_signal_sub订阅了话题“/rcvmap_signal”，同样是状态标志，表示在加载完地图后成功创建了栅格节点地图。

3D2M的节点话题传输图

 三、构建地图

void PCSmapManager::init(ros::NodeHandle& nh)
{
    //所有地图的分辨率由launch文件设定为为0.3
    //sta_threshold统计滤波阈值，如果栅格内的点云数量超过阈值，视为占用
    //debug_output用于输出调试信息
    nh.param("occupancy_resolution", occupancy_resolution, 0.1);
    nh.param("travelmap_resolution", travelmap_resolution, 0.1);
    nh.param("sta_threshold", sta_threshold, 2);
    nh.param("debug_output", debug_output, true);

    //并没有在程序中使用
    nh.param("backcost_step_roh", backcost_step_roh, 1.0);
    nh.param("backcost_roug_roh", backcost_roug_roh, 0.000002);
    nh.param("backcost_tilt_roh", backcost_tilt_roh, 100.0);

    //好像是机器人可主动改变高度的范围，在仿真程序中没有体现
    nh.param("min_height", min_height, 0.3);
    nh.param("max_height", max_height, 0.6);
    nh.param("downproj", downproj, 0);

    //创建空地图
    occupancy_map.reset(new GridMap);
    occupancy_map->grid_pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    occupancy_map->grid_kdtree.reset(new pcl::KdTreeFLANN<pcl::PointXYZ>);
    occupancy_map->grid_resolution = occupancy_resolution;
    occupancy_map->debug_output    = debug_output;

    snowed_occupancy_map.reset(new GridMap);
    snowed_occupancy_map->grid_pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    snowed_occupancy_map->grid_kdtree.reset(new pcl::KdTreeFLANN<pcl::PointXYZ>);
    snowed_occupancy_map->grid_resolution = occupancy_resolution;
    snowed_occupancy_map->debug_output    = debug_output;

    zcost_map.reset(new GridMap);
    zcost_map->grid_pointcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    zcost_map->grid_kdtree.reset(new pcl::KdTreeFLANN<pcl::PointXYZ>);
    zcost_map->grid_resolution = occupancy_resolution;
    zcost_map->debug_output    = debug_output;

    travelcost_map.reset(new TravelGridMap);
    travelcost_map->grid_resolution     = travelmap_resolution;
    travelcost_map->debug_output        = debug_output;

    surfel_map.reset(new SurFelMap);
    surfel_map -> init(nh);


    globalmap_sub        = nh.subscribe("globalmap",1, &PCSmapManager::rcvGlobalMapHandler, this);
    odometry_sub         = nh.subscribe("odom", 1 , &PCSmapManager::rcvOdomHandler, this);
    debug_grad_sub       = nh.subscribe("/renderGrad", 1 , &PCSmapManager::rcvRenderGrad, this); 

    globalmap_vis_pub    = nh.advertise<sensor_msgs::PointCloud2>("globalmap_vis", 10);
    gridmap_vis_pub      = nh.advertise<sensor_msgs::PointCloud2>("gridmap_vis", 10);
    costmap_vis_pub      = nh.advertise<sensor_msgs::PointCloud2>("costmap_vis", 10);
    voxel_vis_pub        = nh.advertise<sensor_msgs::PointCloud2>("voxel_vis", 10);
    rcvmap_signal_pub    = nh.advertise<std_msgs::Empty>("rcvmap_signal", 10);

    debug_grad_pub       = nh.advertise<sensor_msgs::PointCloud2>("grad_vis",10);
}