OccupancyGrid
ROS navigates through the OccupancyGrid, which consists of a yaml format metadata file, and picture format map data file.
-
Map metadata
Map metadata XXX The format of yaml is as follows:image: testmap.pgm resolution: 0.1 origin: [0.0, 0.0, 0.0] occupied_thresh: 0.65 free_thresh: 0.196 negate: 0
Notes are as follows:
Image: Specifies the path of the image file containing occupancy data; It can be an absolute path or an object path relative to the YAML file.
Resolution: map resolution, in meters/pixel.
origin: the two-dimensional pose of the lower left pixel in the figure, such as (x, y, yaw). Yaw rotates counterclockwise (yaw=0 means no rotation). Many parts of the system now ignore the yaw value.
occupied_thresh: if the pixel occupancy rate is greater than this threshold, it is considered to be fully occupied.
free_thresh: if the pixel occupancy is smaller than the threshold, it is considered completely free.
Why the White / black free / occupied semantics should be reversed (Interpretation of thresholds is unaffected)
-
Map data:
Image describes the occupancy status of each unit in the color of the corresponding pixel on the map. White pixels represent freedom, black pixel cells represent occupancy, and cells between two colors represent unknown. Both color and gray images are OK. If it is a color image, the average value of all channels is calculated. The image unit occupancy probability is calculated as follows: OCC = (255 color_avg) / 255.0 (color_avg is the average value of all channels). This formula indicates that the darker the pixel, the higher the occupancy probability, and the whiter the pixel, the lower the occupancy probability.
When ROS messages communicate, the data type is nav_msgs::OccupancyGrid. At this time, the occupancy is expressed as an integer in the range of [0-100]. 0 means completely free, 100 means completely occupied, and the special value - 1 is completely unknown.
data type
nav_msgs/OccupancyGrid
std_msgs/Header header nav_msgs/MapMetaData info int8[] data
notes
Header message header
info map metadata
Data map data
nav_msgs/MapMetaData
time map_load_time
float32 resolution uint32 width uint32 height geometry_msgs/Pose origin
notes
map_load_time map loading time
Resolution map resolution m/cell
Width map width, pixels
Height map height
Origin the origin of the map, that is, the coordinates of pixels (0,0) in the world coordinate system
nav_msgs/GetMap
--- nav_msgs/OccupancyGrid map # acquired map
map_server Feature Pack
file: http://wiki.ros.org/map_server
You can provide a two-dimensional map
1.map_server node
This node provides map data through ROS server.
1.1 usage
rosrun map_server map_server mymap.yaml
1.2 subject of release
map_metadata (nav_msgs/MapMetaData) gets the metadata of the map
map (nav_msgs/OccupancyGrid)
1.3 services
static_map (nav_msgs/GetMap) obtains the map through this service
1.4 parameters
~frame_id (string, default: "map") sets the coordinate system frame id of the map
2. map_saver node
map_ The saver can save the map to disk.
2.1 usage
Map_ The saver obtains the map data and writes it to map by default PGM and map yaml.
rosrun map_server map_saver -f /home/xxx/map/AAA
Use the - f option to specify the storage directory and name of the map.
/home/xxx/map / is the map directory path, and zhizhaokongjian is the map name. After generation, AAA is obtained Yaml and AAA PGM two files
2.2 subscription topics
Map (nav_msgs/OccupancyGrid) gets a map by specifying a topic
Read map files and publish themes and services
Read below yaml and pgm map file, build OccupancyGrid message and publish it as / map topic. The published service is / static of GetMap type_ map. There is already a map_server, what's the use of this program? 1. It can help understand the data type of OccupancyGrid; 2. The map can be set dynamically.
#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);
//Service callback
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<<"Publishing map"<<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;
//Message header
map.header.frame_id="map";
map.header.stamp = ros::Time::now();
cv::FileStorage f_yaml(metaPath,cv::FileStorage::READ);//Read yaml information
map.info.resolution=(float)f_yaml["resolution"]; //Map resolution
cv::FileNode arr = f_yaml["origin"];
cv::FileNodeIterator it=arr.begin(), it_end = arr.end();
map.info.origin.position.x=(float)( *it );it++;//Set map origin
map.info.origin.position.y=(float)( *it );it++;
map.info.origin.position.z=(float)( *it );
f_yaml.release();
//Read map image file
cv::Mat image = cv::imread(imgPath, -1);
vector<signed char> vec;
//Convert the image file to vector and scale the data range to 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<<"Occupied grid meta information:"<<endl;
cout<<map<<endl;
map.data = vec;
return map;
}Note that there is a big hole here, that is, the array in the OccupancyGrid should be in reverse row and column order when storing pictures. Because when the client reads, it reads in row order and column reverse order.
The dependent function packages are: geometry_msgs，roscpp，nav_msgs. To read the yaml configuration file using OpenCV here, you need to add% YAML:1.0 in the first line of the file. It may be better to read it by other methods.
After publishing, view the published map in rviz:
Published maps for move_base path planning.