路径规划离不开地图，其中真实地图，栅格地图和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 x1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,current_costmap_ptr_->info.origin.position.y, //yaml origin y1.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 xmap_x_upper_ = x_upper;map_y_lower_ = y_lower; //yaml origin ymap_y_upper_ = y_upper;STATE_GRID_RESOLUTION_ = state_grid_resolution; // yaml里的MAP_GRID_RESOLUTION_ = map_grid_resolution; //0.2STATE_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对应的真实世界的长宽。
