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

1.目的

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

2.编写源文件

#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"
#include <iostream>
#include <string>
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<std::pair<uint32_t, uint32_t>> obsVec = ....;
    std::vector<int8_t> 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<nav_msgs::OccupancyGrid>("/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保存栅格地图为图片

<launch>
  	<node pkg="map_to_costmap" type="map_tp_occupy" name="map_tp_occupy"/>
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find map_to_costmap)/config/map.rviz"/>
    <arg name="filename" value="$(find map_to_costmap)/maps/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>