cartographer发布尺寸不变的地图

void mapConvert(const nav_msgs::OccupancyGrid& map_old , nav_msgs::OccupancyGrid& map_new ){
map_new.header.stamp = ros::Time::now();
map_new.header.frame_id = “/map”;
map_new.info.map_load_time = ros::Time::now();
map_new.info.resolution = 0.05 ;
map_new.info.width = 400 ;
map_new.info.height = 400 ;
map_new.info.origin.position.x = -10.0 ;
map_new.info.origin.position.y = -10.0 ;
map_new.info.origin.position.z = 0.0 ;
map_new.info.origin.orientation.x = 0.0 ;
map_new.info.origin.orientation.y = 0.0 ;
map_new.info.origin.orientation.z = 0.0 ;
map_new.info.origin.orientation.w = 1.0 ;

	for(int i = 0 ; i < 400 * 400 ; i++){
		map_new.data.push_back( -1 );
	}

    for(int i = 0 ; i < map_old.data.size() ; i++ ){

      if( map_old.data.at(i) == -1 ){

      }
      else{
        unsigned int my_old = i / map_old.info.width  ;
        unsigned int mx_old = i - my_old * map_old.info.width  ;

        double   wx = map_old.info.origin.position.x  + (mx_old + 0.5) * map_old.info.resolution ;
        double   wy = map_old.info.origin.position.y  + (my_old + 0.5) * map_old.info.resolution ;

        int mx_new = abs( (int)((wx - map_new.info.origin.position.x ) /  map_new.info.resolution) );
        int my_new = abs( (int)((wy - map_new.info.origin.position.y) /  map_new.info.resolution) );

        unsigned int index_new = my_new * map_new.info.width + mx_new;
        map_new.data.at( index_new ) = map_old.data.at(i)  ;
      }
    }

}