1. Hàm Cập Nhật Biên Giới

Hàm updateBounds xác định tọa độ biên giới bản đồ cần cập nhật:

if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
useExtraBounds(min_x, min_y, max_x, max_y);

Quy trình thu thập dữ liệu cảm biến:

std::vector<Observation> marking_data, clearing_data;
bool current_state = getMarkingObservations(marking_data) && 
                    getClearingObservations(clearing_data);

Hai bộ đệm sử dụng chung đối tượng dữ liệu:

observation_buffers_.push_back(
    boost::make_shared<ObservationBuffer>(/*params*/));

if (marking) marking_buffers_.push_back(observation_buffers_.back());
if (clearing) clearing_buffers_.push_back(observation_buffers_.back());

Xử lý không gian tự do:

for (auto& obs : clearing_data) {
    raytraceFreespace(obs, min_x, min_y, max_x, max_y);
}

Gán giá trị ô chướng ngại vật:

for (const auto& sensor_reading : marking_data) {
    const auto& cloud = *sensor_reading.point_cloud_;
    double max_dist_sq = sensor_reading.obstacle_range_ * sensor_reading.obstacle_range_;
    
    for (sensor_msgs::PointCloud2ConstIterator<float> it(cloud, "x"); it != it.end(); ++it) {
        if (it.z > max_obstacle_height_) continue;
        
        double dist_sq = pow(it.x - sensor_reading.origin_.x, 2) + 
                         pow(it.y - sensor_reading.origin_.y, 2);
        if (dist_sq >= max_dist_sq) continue;

        unsigned int grid_x, grid_y;
        if (worldToMap(it.x, it.y, grid_x, grid_y)) {
            costmap_[getIndex(grid_x, grid_y)] = LETHAL_OBSTACLE;
        }
    }
}
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);

2. Hàm Cập Nhật Chi Phí

Thiết lập vùng robot chiếm dụng:

if (footprint_cleering_enabled_) {
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
}

Kiểm tra đa giác trong bản đồ:

std::vector<MapCell> polygon_cells;
for (const auto& point : polygon) {
    MapCell cell;
    if (!worldToMap(point.x, point.y, cell.x, cell.y)) return false;
    polygon_cells.push_back(cell);
}

Gán giá trị ô:

for (const auto& cell : polygon_cells) {
    costmap_[getIndex(cell.x, cell.y)] = FREE_SPACE;
}

Tích hợp vào bản đồ chính:

switch (fusion_method_) {
    case OVERWRITE: 
        mergeWithOverwrite(master_grid, x_min, y_min, x_max, y_max);
        break;
    case MAXIMUM:
        mergeWithMax(master_grid, x_min, y_min, x_max, y_max);
        break;
}

Ví dụ tích hợp:

void mergeWithOverwrite(Costmap2D& main_grid, int x_min, int y_min, int x_max, int y_max) {
    unsigned char* master_data = main_grid.getCharMap();
    for (int j = y_min; j < y_max; ++j) {
        int idx = main_grid.getSizeInCellsX() * j + x_min;
        for (int i = x_min; i < x_max; ++i) {
            if (costmap_[idx] != NO_INFORMATION) 
                master_data[idx] = costmap_[idx];
            ++idx;
        }
    }
}

3. Thuật Toán Dò Tia

Khởi tạo tọa độ cảm biến:

double sensor_x = obs.origin_.x;
double sensor_y = obs.origin_.y;
const auto& cloud = *obs.point_cloud_;

Xử lý điểm ngoài biên:

for (sensor_msgs::PointCloud2ConstIterator<float> it(cloud, "x"); it != it.end(); ++it) {
    double dx = it.x - sensor_x;
    double dy = it.y - sensor_y;
    
    if (it.x < map_origin_x_) {
        double ratio = (map_origin_x_ - sensor_x) / dx;
        it.x = map_origin_x_;
        it.y = sensor_y + dy * ratio;
    }
    // Xử lý các biên khác tương tự
}

Thực hiện dò tia:

unsigned int max_ray_cells = cellDistance(obs.raytrace_range_);
auto marker = [&](unsigned int idx){ costmap_[idx] = FREE_SPACE; };
raytraceLine(marker, x_sensor, y_sensor, x_target, y_target, max_ray_cells);

4. Cập Nhật Hình Dạng Robot

Biến đổi tọa độ chân robot:

if (!footprint_clearing_enabled_) return;
transformFootprint(robot_x, robot_y, robot_yaw, 
                   getFootprint(), transformed_footprint_);

for (const auto& point : transformed_footprint_) {
    adjustBounds(point.x, point.y, min_x, min_y, max_x, max_y);
}

Tạo chân robot dạng tròn:

std::vector<geometry_msgs::Point> createCircularFootprint(double radius) {
    std::vector<geometry_msgs::Point> points;
    const int num_points = 16;
    for (int i = 0; i < num_points; ++i) {
        double angle = i * 2 * M_PI / num_points;
        geometry_msgs::Point pt;
        pt.x = cos(angle) * radius;
        pt.y = sin(angle) * radius;
        points.push_back(pt);
    }
    return points;
}

Biến đổi tọa độ:

void transformFootprint(double x, double y, double theta, 
                       const std::vector<geometry_msgs::Point>& footprint,
                       std::vector<geometry_msgs::Point>& output) {
    output.clear();
    double cos_theta = cos(theta);
    double sin_theta = sin(theta);
    for (const auto& pt : footprint) {
        geometry_msgs::Point new_pt;
        new_pt.x = x + (pt.x * cos_theta - pt.y * sin_theta);
        new_pt.y = y + (pt.x * sin_theta + pt.y * cos_theta);
        output.push_back(new_pt);
    }
}