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-100for (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的路径规划。

ROS-OccupancyGrid学习笔记相关推荐

ROS中的roslaunch命令和launch文件（ROS入门学习笔记四）
ROS中的基本对象和概念学习笔记(ROS入门学习笔记一) ROS中创建工作区和包(ROS入门学习笔记二) ROS功能包中CMakeLists.txt的说明(ROS入门学习笔记三) 1.roslaunc ...
ekf pose使用方法 ros_ubuntu16.04下ROS操作系统学习笔记（二）
做SLAM的硬件要求(不一定是必须的,看包和库的依赖): (1):差分轮式机器人,可以使用Twist速度指令控制,需要线速度和角速度. (2):需要激光雷达.深度摄像头等测距设备,可以获取环境深度信息 ...
opt文件夹下没有ros_ubuntu16.04下ROS操作系统学习笔记（二）
做SLAM的硬件要求(不一定是必须的,看包和库的依赖): (1):差分轮式机器人,可以使用Twist速度指令控制,需要线速度和角速度. (2):需要激光雷达.深度摄像头等测距设备,可以获取环境深度信息 ...
ros android 方案,ros android_core学习笔记
本笔记以app的页面为导向逐步介绍连接master--启动服务的过程 1. 连接master 当前活动继承了RosActivity类: public class MainActivity extend ...
ROS入门学习笔记|话题发布与订阅
文章目录一.工作空间 1.创建一个名称为sor_ws的工作空间 2.编译工作空间 3.创建功能包二.自定义话题消息 1.定义msg文件 2.配置package.xml和CMakeLists.txt ...
【ROS】学习笔记一 ubuntu16.04下vs code配置ros环境并编写helloworld程序
[ROS]ubuntu16.04下vs code配置ros环境并编写helloworld程序一.vs code下载 1.相关配置要求: (1)系统为ubuntu16.04 (2)已安装好kineti ...
【ros】初学ROS的学习笔记——创建Publisher
发布者Publisher的创建本文代码为c++,系统为ubuntu20.04 前言依据材料:古月ros入门21讲 . ROS机器人开发实践要求:已有合适的工作空间文章目录发布者Publish ...
ros indigo 学习笔记
关注同名微信公众号"混沌无形",有趣好文! 1.任务执行(task executive)的特点: 任务优先级(task priorities):各项任务有优先级,高级任务可以打断较 ...
ROS基础学习笔记（五）
记录数据和回放: 记录数据: 1.启动案例程序: roscore rosrun turtlesim turtlesim_node rosrun turtlesim turtle ...
ROS学习笔记（八）： ROS通信架构
ROS学习笔记(八): ROS通信架构文章目录 01 Node & Master 1.1 Node 1.2 Master 1.3 启动master和node 1.4 rosrun和rosno ...

ROS-OccupancyGrid学习笔记