路径规划关于地图的整理

路径规划离不开地图，其中真实地图，栅格地图和RVIZ之间Grid显示之间很混乱，还有各个原点位置显示，不弄清发现map在rviz里显示老是偏的，专门学习记录一下。

RVIZ里Grid的全局坐标系原点，在默认在栅格中间，右手系（食指x,中止y）。
map.yaml origin: [x,y,z] 表示pgm地图 右上角 在Grid位置,z是旋转角度

origin: [50,50,0] 样子：

注意：Grid栅格密度、大小和地图密度无关

混合A星对地图处理：

 const double map_resolution = 0.2;
                kinodynamic_astar_searcher_ptr_->Init(
                        current_costmap_ptr_->info.origin.position.x, //yaml origin x
                        1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,
                        current_costmap_ptr_->info.origin.position.y, //yaml origin  y
                        1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,
                        current_costmap_ptr_->info.resolution,
                        map_resolution=0.2
                );
                
void HybridAStar::Init(double x_lower, double x_upper, double y_lower, double y_upper,
                       double state_grid_resolution, double map_grid_resolution) {
    SetVehicleShape(vehicle_length_, vehicle_width_, vehicle_rear_axle_to_tailstock_);

    map_x_lower_ = x_lower;  //yaml origin x
    map_x_upper_ = x_upper;
    map_y_lower_ = y_lower;  //yaml origin y
    map_y_upper_ = y_upper;
    STATE_GRID_RESOLUTION_ = state_grid_resolution;  // yaml里的
    MAP_GRID_RESOLUTION_ = map_grid_resolution;  //0.2

    STATE_GRID_SIZE_X_ = std::floor((map_x_upper_ - map_x_lower_) / STATE_GRID_RESOLUTION_); //
    STATE_GRID_SIZE_Y_ = std::floor((map_y_upper_ - map_y_lower_) / STATE_GRID_RESOLUTION_); //

    MAP_GRID_SIZE_X_ = std::floor((map_x_upper_ - map_x_lower_) / MAP_GRID_RESOLUTION_);
    MAP_GRID_SIZE_Y_ = std::floor((map_y_upper_ - map_y_lower_) / MAP_GRID_RESOLUTION_);

因为是右手系，map在RVIZ里是朝左下增大。 所以origin x,y就是lower
最大就是 current_costmap_ptr_->info.width （栅格宽 具体看Grid坐标朝向对应即可）* current_costmap_ptr_->info.resolution （实际分辨率）
info.resolution=0.5 表示一个栅格代表现实世界0.5m.
一乘就得到了pgm对应的真实世界的长宽。