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