ROS OccupancyGrid使用说明

博客转自：chenjianqu

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
文档API

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

变量注释

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

nav_msgs/MapMetaData

文档API

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

文档API

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

map_server功能包

可以提供一个二维地图, 文档API

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

/home/xxx/map/为地图目录路径，AAA为地图名称，生成后得到AAA.yaml和AAA.pgm两个文件
使用-f选项为指定地图的存放目录和名称。
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(
    "/home/ke/map/cloister_gmapping.pgm",
    "/home/ke/map/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(
	  "/home/ke/map/map.pgm",
	  "/home/ke/map/map.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的路径规划。