layers/src/pointMap_layer.cpp
// update bounds (base function in costmap class)
update bounds (base function in costmap class)
update bounds ( base function in costmap class )
void PointMapLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
    if (rolling_window_)
        updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
    if (!enabled_)
        return;
    useExtraBounds(min_x, min_y, max_x, max_y);
    for (int i = 0; i < ob_pointCloud.points.size(); i++)
    {
        auto& point = ob_pointCloud.points[i];
        unsigned int mx, my;
        double px = point.x;
        double py = point.y;
        if (!worldToMap(px, py, mx, my))
        {
            ROS_DEBUG("Computing map coords failed");
            continue;
        }
        unsigned int index = getIndex(mx, my);
        if(point.travers > 0.1){
            costmap_[index] = FREE_SPACE;
        }else{
            costmap_[index] = LETHAL_OBSTACLE;
        }
        touch(px, py, min_x, min_y, max_x, max_y);
    }
    updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void PointMapLayer :: updateBounds ( double robot_x , double robot_y , double robot_yaw , double * min_x , double * min_y , double * max_x , double * max_y ) { if ( rolling_window_ ) updateOrigin ( robot_x - getSizeInMetersX ( ) / 2 , robot_y - getSizeInMetersY ( ) / 2 ) ; if ( ! enabled_ ) return ; useExtraBounds ( min_x , min_y , max_x , max_y ) ; for ( int i = 0 ; i < ob_pointCloud . points . size ( ) ; i ++ ) { auto & point = ob_pointCloud . points [ i ] ; unsigned int mx , my ; double px = point . x ; double py = point . y ; if ( ! worldToMap ( px , py , mx , my ) ) { ROS_DEBUG ( " " ) ; continue ; } unsigned int index = getIndex ( mx , my ) ; if ( point . travers > 0.1 ) { costmap_ [ index ] = FREE_SPACE ; } else { costmap_ [ index ] = LETHAL_OBSTACLE ; } touch ( px , py , min_x , min_y , max_x , max_y ) ; } updateFootprint ( robot_x , robot_y , robot_yaw , min_x , min_y , max_x , max_y ) ; }
update bounds (base function in costmap class)
update bounds ( base function in costmap class )
// iterate all points // get index of the costmap using raw point xy // directly update costmap using obstacle value given by GEM // update map