OccupancyGrid

    ROS通过OccupancyGrid(占据网格)进行导航，OccupancyGrid由一个.yaml格式的元数据文件，和图片格式的地图数据文件组成。

• 地图元数据
  地图元数据 xxx.yaml 的格式如下：

  image: testmap.pgm
  resolution: 0.1
  origin: [0.0, 0.0, 0.0]
  occupied_thresh: 0.65
  free_thresh: 0.196
  negate: 0

注释如下：

image: 指定包含occupancy data的image文件路径; 可以是绝对路径，也可以是相对于YAML文件的对象路径 。
resolution: 地图分辨率，单位是meters/pixel 。
origin: 图中左下角像素的二维位姿，如（x，y，yaw），yaw逆时针旋转(yaw=0表示没有旋转)。系统的很多部分现在忽略yaw值。
occupied_thresh: 像素占用率大于这个阈值则认为完全占用。
free_thresh: 像素占用率比该阈值小被则认为完全自由。
negate: 无论白色或黑色，占用或自由，语义应该是颠倒的（Whether the white/black free/occupied semantics should be reversed (interpretation of thresholds is unaffected))

• 地图数据：

Image 描述了地图上每个单元在相应像素的颜色中的占用状态。白色像素表示自由，黑色像素格表示占用，两种颜色之间的单元表示未知。彩色和灰度图像都可以，如果是彩色图像，则计算所有通道的平均值。图像单元占用概率的计算如下：occ = (255-color_avg)/255.0 （color_avg是所有通道的平均值），该公式表示像素点越黑占用概率越高，越白占用概率越低。

    当ROS消息通信时，数据类型为nav_msgs::OccupancyGrid，此时占用度被表示为范围为[0-100]的整数，0的意思完全是自由的，100的意思完全占用，特殊值-1完全未知。

数据类型

nav_msgs/OccupancyGrid

• 文档：http://docs.ros.org/kinetic/api/nav_msgs/html/msg/OccupancyGrid.html

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data

注释

header 消息头
info 地图元数据
data 地图数据

nav_msgs/MapMetaData

• 文档：http://docs.ros.org/kinetic/api/nav_msgs/html/msg/MapMetaData.html

time map_load_time

float32 resolution
uint32 width
uint32 height
geometry_msgs/Pose origin

注释

map_load_time 地图加载时间
resolution 地图分辨率m/cell
width 地图宽度，像素数
height 地图高度
origin 地图的原点，即像素(0,0)在世界坐标系的坐标

nav_msgs/GetMap

• 文档：http://docs.ros.org/kinetic/api/nav_msgs/html/srv/GetMap.html

---
nav_msgs/OccupancyGrid map #获取的地图

map_server功能包

文档：http://wiki.ros.org/map_server
可以提供一个二维地图

1.map_server节点

该节点通过ROS 服务器方式提供地图数据。

1.1 用法

rosrun map_server map_server mymap.yaml

1.2 发布的主题

map_metadata (nav_msgs/MapMetaData) 获取地图的metadata
map (nav_msgs/OccupancyGrid) 通过获取地图

1.3 服务

static_map (nav_msgs/GetMap) 通过该服务来获取地图

1.4 参数

~frame_id (string, default: "map") 设置地图的坐标系帧id

2. map_saver 节点

map_saver可以把地图保存到磁盘。

2.1 用法

 Map_saver获取地图数据，默认写到map.pgm和map.yaml。

rosrun map_server map_saver -f /home/xxx/map/AAA

使用-f选项为指定地图的存放目录和名称。
/home/xxx/map/为地图目录路径，zhizaokongjian为地图名称，生成后得到AAA.yaml和AAA.pgm两个文件

2.2 订阅话题

map (nav_msgs/OccupancyGrid) 通过指定话题来获取地图

读取地图文件并发布主题和服务

    下面读取.yaml和.pgm地图文件，构建OccupancyGrid消息， 并发布为/map主题．发布的服务为GetMap类型的/static_map。已经有map_server，那这个程序有什么用？1.可以帮助理解OccupancyGrid这个数据类型；2.可以动态设置地图。

#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <string>

using namespace std;

nav_msgs::OccupancyGrid BuildMap(const string& imgPath,const string& metaPath);

//服务回调
bool ServiceCallBack(nav_msgs::GetMap::Request &req,nav_msgs::GetMap::Response &res)
{
    res.map=BuildMap(
    "/media/chen/chen/Robot/projects_ros_test/test/src/mrobot_navigation/maps/cloister_gmapping.pgm",
    "/media/chen/chen/Robot/projects_ros_test/test/src/mrobot_navigation/maps/cloister_gmapping1.yaml"
    );
    return true;
}

int main(int argc, char * argv[]) 
{
	ros::init(argc, argv, "map_gen_node");

	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(
	  "/media/chen/chen/Robot/projects_ros_test/test/map.pgm",
	  "/media/chen/chen/Robot/projects_ros_test/test/map1.yaml"
	);

	cout<<"正在发布地图"<<endl;

	while (ros::ok())
	{
		pub.publish(map);
	}

	ros::shutdown();
	return 0;
}


nav_msgs::OccupancyGrid BuildMap(const string& imgPath,const string& metaPath)
{
	nav_msgs::OccupancyGrid map;
	//消息头
	map.header.frame_id="map";
	map.header.stamp = ros::Time::now();

	cv::FileStorage f_yaml(metaPath,cv::FileStorage::READ);//读取yaml信息
	map.info.resolution=(float)f_yaml["resolution"]; //地图分辨率

	cv::FileNode arr = f_yaml["origin"];
	cv::FileNodeIterator it=arr.begin(), it_end = arr.end();
	map.info.origin.position.x=(float)( *it );it++;//设置地图原点
	map.info.origin.position.y=(float)( *it );it++;
	map.info.origin.position.z=(float)( *it );

	f_yaml.release();

	//读取地图图像文件
	cv::Mat image = cv::imread(imgPath, -1);
	vector<signed char> vec;
	//将图像文件转换为vector，并将数据范围缩放至0-100
        for (int i = image.rows-1; i >=0 ; i--) {
            uchar *imageRow = image.ptr(i);
            for (int j = 0;j<image.cols; j++)
                vec.push_back((char)((255-imageRow[j])*100/255));
        }
	map.info.width=image.cols;
	map.info.height=image.rows;

	cout<<"占据网格元信息："<<endl;
	cout<<map<<endl;

	map.data = vec;

	return map;
}

    注意这里有一个很坑的地方，那就是OccupancyGrid里面的数组，存入图片的时候，应该是行逆序、列顺序的。因为客户端读取的时候，是按照行顺序，列逆序来读取的。

    依赖的功能包有：geometry_msgs，roscpp，nav_msgs．这里使用OpenCV读取yaml配置文件，需要在文件首行加入 %YAML:1.0 ，用其他方法读可能会好点。

    发布后，在rviz中查看发布的地图：

    发布的地图用于move_base的路径规划。