将ros地图平移转换一下得到新地图

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <vector>
#include <utility>

ros::Publisher obstacle_map_pub; // 障碍物地图发布者

std::vector<std::pair<double, double>> obstacle_points; // 存储障碍物点的向量
nav_msgs::OccupancyGrid obstacle_map;

void MapCallback(const nav_msgs::OccupancyGrid& map)
{
    obstacle_points.clear(); // 清空障碍物点，避免重复保存

    int size_x = map.info.width;
    int size_y = map.info.height;

    for (int j = 0; j < size_y; ++j)
    {
        for (int i = 0; i < size_x; ++i)
        {
            int index = j * size_x + i;
            if (map.data[index] > 0) // 如果这个栅格被占据
            {
                double x = map.info.origin.position.x + (i + 0.5) * map.info.resolution;
                double y = map.info.origin.position.y + (j + 0.5) * map.info.resolution;
                // 对坐标进行偏移
                x += 0.5;
                y -= 0.25;
                obstacle_points.push_back(std::make_pair(x, y));
            }
        }
    }
    std::cout<<"obstacle_points"<<obstacle_points.size()<<std::endl;
    // 发布新的障碍物地图消息
    
    obstacle_map.header = map.header;
    obstacle_map.info = map.info;
    obstacle_map.data.resize(size_x * size_y, 0); // 初始化地图数据为0

    for (const auto& obstacle_point : obstacle_points) {
        // 计算障碍物点在地图中的索引
        int map_index_x = (obstacle_point.first - map.info.origin.position.x) / map.info.resolution;
        int map_index_y = (obstacle_point.second - map.info.origin.position.y) / map.info.resolution;
        int map_index = map_index_y * size_x + map_index_x;
        // 将障碍物点标记为被占据
        if (map_index >= 0 && map_index < size_x * size_y) {
            obstacle_map.data[map_index] = 100; // 这里将障碍物点标记为100，可以根据需要修改
        }
    }

   
}

int main(int argc, char **argv)  
{
    ROS_INFO("Starting map saver node");

    ros::init(argc, argv, "test_map_saver");
    ros::NodeHandle node; 
    ros::Subscriber map_sub = node.subscribe("map", 1, MapCallback);
    obstacle_map_pub = node.advertise<nav_msgs::OccupancyGrid>("obstacle_map", 1); // 创建障碍物地图发布者


ros::Rate rate(10);
    while (ros::ok())
    {

        if (obstacle_map_pub.getNumSubscribers() > 0) {
         obstacle_map_pub.publish(obstacle_map); // 发布障碍物地图消息
        }



    ros::spinOnce();
        rate.sleep();

    }

    
  
    return 0;
}