ROS OccupancyGrid使用说明

ROS OccupancyGrid使用说明
转载自：http://t.zoukankan.com/flyinggod-p-13463249.html
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
#include

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中查看发布的地图：

图片1.jpg

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