ROS-OccupancyGrid学习笔记

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完全未知。


数据类型

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

注释

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

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

图片1.jpg


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







下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于