Bayesian Generalized Kernel Inference for Terrain Traversability Mapping代码解读（二）

下面对traversability_map节点进行学习。

首先看main函数，创建了一个节点。

int main(int argc, char** argv){

    ros::init(argc, argv, "traversability_mapping");
    
    TraversabilityMapping tMapping;

    std::thread predictionThread(&TraversabilityMapping::TraversabilityThread, &tMapping);

    ROS_INFO("\033[1;32m---->\033[0m Traversability Mapping Started.");
    ROS_INFO("\033[1;32m---->\033[0m Traversability Mapping Scenario: %s.", 
        urbanMapping == true ? "\033[1;31mUrban\033[0m" : "\033[1;31mTerrain\033[0m");

    ros::spin();

    return 0;
}

看TraversabilityMapping的构造函数，对滤波后的点云进行订阅，发布局部占据栅格地图和高程图。

TraversabilityMapping():
        nh("~"),
        pubCount(1),
        mapArrayCount(0){
        // subscribe to traversability filter
        subFilteredGroundCloud = nh.subscribe<sensor_msgs::PointCloud2>("/filtered_pointcloud", 5, &TraversabilityMapping::cloudHandler, this);
        // publish local occupancy and elevation grid map
        pubOccupancyMapLocal = nh.advertise<nav_msgs::OccupancyGrid> ("/occupancy_map_local", 5);
        pubOccupancyMapLocalHeight = nh.advertise<elevation_msgs::OccupancyElevation> ("/occupancy_map_local_height", 5);
        // publish elevation map for visualization
        pubElevationCloud = nh.advertise<sensor_msgs::PointCloud2> ("/elevation_pointcloud", 5);

        allocateMemory(); 
    }

接下来看回调函数，这里首先锁定了一个线程，将当前的点云消息接收之后，对高程地图进行更新，再发布。

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
        // Lock thread
        std::lock_guard<std::mutex> lock(mtx);
        // Get Robot Position
        if (getRobotPosition() == false) 
            return;
        // Convert Point Cloud
        pcl::fromROSMsg(*laserCloudMsg, *laserCloud);
        // Register New Scan
        updateElevationMap();
        // publish local occupancy grid map
        publishMap();
    }

地图更新

    void updateElevationMap(){
        int cloudSize = laserCloud->points.size();
        for (int i = 0; i < cloudSize; ++i){
            laserCloud->points[i].z -= 0.2; // for visualization
            updateElevationMap(&laserCloud->points[i]);
        }
    }

具体更新如下，包括高度，占据，以及观测次数的更新。

 void updateElevationMap(PointType *point){
        // Find point index in global map
        grid_t thisGrid;
        if (findPointGridInMap(&thisGrid, point) == false) return;
        // Get current cell pointer
        mapCell_t *thisCell = grid2Cell(&thisGrid);
        // update elevation
        updateCellElevation(thisCell, point);
        // update occupancy
        updateCellOccupancy(thisCell, point);
        // update observation time
        updateCellObservationTime(thisCell);
    }

更新了之后就是发布地图了

    void publishMap(){
        // Publish Occupancy Grid Map and Elevation Map
        pubCount++;
        if (pubCount > visualizationFrequency){
            pubCount = 1;
            publishLocalMap();
            publishTraversabilityMap();
        }
    }