自定义地图并发布为OccupancyGrid显示

1.目的

将自定义的地图，转换成栅格地图发布，并在rviz中显示。然后保存为pgm和yaml。

2.编写源文件

#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"
#include 
#include 
using namespace std;

//自定义地图的函数，可以通过各种方式，对nav_msgs::OccupancyGrid.data进行操作
nav_msgs::OccupancyGrid BuildMap()
{
	nav_msgs::OccupancyGrid rosMap;
	//消息头
	rosMap.header.frame_id="map";
	rosMap.header.stamp = ros::Time::now();
    rosMap.info.resolution = 0.03;
    rosMap.info.origin.position.x = 0.0;
    rosMap.info.origin.position.y = 0.0;
    rosMap.info.origin.position.z = 0.0;
    rosMap.info.origin.orientation.x = 0.0;
    rosMap.info.origin.orientation.y = 0.0;
    rosMap.info.origin.orientation.z = 0.0;
    rosMap.info.origin.orientation.w = 1.0;

    rosMap.info.width = 150; //sizeX_
    rosMap.info.height = 150;//sizeY_
    int sizeX = 150;
    int sizeY = 150;
    //也可以读取图片获取像素点
    //获取障碍物点obsVec。
	std::vector> obsVec = ....;
    std::vector charMap;
    charMap.resize(sizeX*sizeY);
    for(auto p : obsVec) {
        int index = sizeX * p.first + p.second;
        charMap[index] = 100;//障碍物
    }
    rosMap.data = charMap;
    return rosMap
}

//服务
bool ServiceCallBack(nav_msgs::GetMap::Request &req,nav_msgs::GetMap::Response &res)
{
    res.map = BuildMap();
    return true;
}

int main(int argc, char * argv[]) 
{
	ros::init(argc, argv, "map_to_occupy");
	ros::NodeHandle nh;
    //话题
	ros::Publisher pub = nh.advertise("/map", 1);
   	//服务
    ros::ServiceServer serv = nh.advertiseService("/static_map",ServiceCallBack);
	nav_msgs::OccupancyGrid map=BuildMap();
	while (ros::ok())
	{
		pub.publish(map);
	}
	ros::shutdown();
	return 0;
}

3.编写launch文件

启动map生成节点，启动rviz，启动map_saver保存栅格地图为图片