机器人控制算法—如何使用C++读取pgm格式的栅格地图并转化为ROS地图格式的data?

1.Introduction

近期正在做全局规划+局部动态规划的项目，目前遇到的问题是，我们如何利用C++处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理，这样更好地在非ROS平台下移植。

2.Main

如下几条信息需要了解：
（1）data[]是按照那张地图图片的自底向上，自左至右逐个像素点存储的.
(2) 在使用二维地图定位导航时，建好的地图文件中包括$map.pgm$和$map.yaml$.其中.yaml文件如下:

image: map.pgm　　#文件名
resolution: 0.050000　　#地图分辨率 单位：米/像素
origin: [-49.0286, -107.401, 0.0]   #图像左下角在地图坐标下的坐标
negate: 0    #是否应该颠倒 白：自由/黑：的语义(阈值的解释不受影响)
occupied_thresh: 0.65   #占用概率大于此阈值的像素被认为已完全占用
free_thresh: 0.196   #用率小于此阈值的像素被认为是完全空闲的

需要注意的是origin: [-49.0286, -107.401, 0.0] #图像左下角在地图坐标下的坐标，我们后续利用这条信息，建立像素与世界坐标系之间的关系。
(3)实际上，我们在路径规划实施过程中，是接收到地图像素信息data[],(一维数组)，然后将其复原为原来的像素坐标，再进行路径规划处理。
data[]复原成地图图片像素坐标关系为：

 for(int i = 0; i<map_info_width*map_info_height; i++){x = i%map_info_width;  //还原为像素坐标y = i/map_info_width;  //还原为像素坐标if(data[i] != 0){   cout<<"obstacle:"<<endl;//PG.map_generator_.addCollision({x, y}, 3);PG.map_generator_.addCollision({x, y}, 3);}cout<<endl;}   

(4) 由地图坐标->图像像素坐标
基于地图的坐标转换到图像坐标系上
$w_x{w}_y$表示地图坐标系下的坐标，resolution为分辨率,则:

image_x = (wx - origin_x) / resolution
image_y = (wy - origin_y) / resolution

（5）由图像像素坐标->地图坐标
image_x,image_y表示在图像像素坐标系中的坐标
$w_x{w}_y$表示地图坐标系下的坐标，resolution为分辨率,则：

wx=image_x*resolution+origin_x
wy=image_y*resolution+origin_y

3.Examples

我们举了一个从地图pgm读取到处理成目标地图数据格式data[] 的例子。

int main(int argc, char **argv)
{PathGenerator PG;//Read pgmcv::Mat m4 = cv::imread("/home/juchunyu/20231013/globalPlanner/AStar-ROS/map/map.pgm",cv::IMREAD_GRAYSCALE);cout << "图像宽为：" << m4.cols << "\t高度为：" << m4.rows << "\t通道数为：" << m4.channels() << endl;/*for (int r = 0; r < m4.rows; ++r) {for (int c = 0; c < m4.cols; ++c) {int data = m4.at<unsigned char>(r, c);}}cout<<"0"<<endl;*/// Round goal coordinatefloat goal_x            = 10;//round(goal_msg->pose.position.x*10)/10;float goal_y            = 10;//round(goal_msg->pose.position.y*10)/10;double origin[3]        = {-9.500000, -10.000000, 0.0};double  occupied_thresh =  0.65;double  free_thresh     =  0.196;int Occupy              = 1;int NoOccupy            = 0;double map_resolution   = 0.05;/*vector<vector<int>> maze = {{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },{ 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1 },{ 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1 },{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1 },{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1 },{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 },{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }};*/vector<vector<int>> maze = {{ 0, 0, 0, 0, 0, 0, 0 },{ 0, 0, 0, 1, 0, 0, 0 },{ 0, 0, 0, 0, 0, 0, 0 },{ 0, 0, 0, 0, 0, 0, 0 },{ 0, 0, 0, 0, 0, 0, 0 },{ 0, 0, 0, 0, 0, 0, 0 }};vector<int> data;for(int i = m4.rows-1;i >= 0;i--){for(int j = 0;j < m4.cols;j++){if(m4.at<unsigned char>(i,j)/255 > free_thresh){data.push_back(Occupy);} else {data.push_back(NoOccupy);}}}/*int num = 0;for(int i =maze.size()-1;i >= 0;i--){for(int j = 0;j < maze[0].size();j++){num++;if(maze[i][j] > free_thresh){data.push_back(Occupy);} else {data.push_back(NoOccupy);}}}cout<<"cishu"<<num<<endl;*/for(int i = 0;i<data.size();i++){cout<<data[i]<<" ";}cout<<endl;//cout<<"maze.size()="<<maze.size()<<endl;//cout<<"maze[0].size()"<<maze[0].size()<<endl;//cv::imshow("res_mat", m4);//cv::waitKey(0);// map_exsit_ = false;//map_info_ = map_msg->info;//int  map_info_width  = maze[0].size();//m4.cols;//int  map_info_height = maze.size();//m4.rows;int  map_info_width  = m4.cols;int  map_info_height = m4.rows;// Generate Map, OptionsPG.map_generator_.setWorldSize({map_info_width, map_info_height}); //{x, y}PG.map_generator_.setHeuristic(AStar::Heuristic::manhattan);PG.map_generator_.setDiagonalMovement(true);cout<<"-3"<<endl;// Add Wallint x, y;for(int i = 0; i<map_info_width*map_info_height; i++){x = i%map_info_width;y = i/map_info_width;cout<<"i"<<i<<endl;cout<<"sum:"<<map_info_width*map_info_height<<endl;double v = double(i/(map_info_width*map_info_height));cout<<v<<"%"<<endl;if(data[i] != 0){   cout<<"obstacle:"<<endl;//PG.map_generator_.addCollision({x, y}, 3);PG.map_generator_.addCollision({x, y}, 3);cout<<"("<<x<<","<<y<<")"<<" ";}cout<<endl;}   cout<<"-2"<<endl; // Remmaping coordinateAStar::Vec2i target;target.x = 162;//6;//2;//161;//(goal_x - origin[0]) / map_resolution;target.y = 105;//3;//9;//112; //(goal_y - origin[1]) / map_resolution;AStar::Vec2i source;source.x = 94;//0;//94;//(0 - origin[0]) /  map_resolution;source.y = 99;//99;//(0 - origin[1]) / map_resolution;cout<<"1"<<endl;// Find Pathauto path =  PG.map_generator_.findPath(source, target);cout<<"2"<<endl;//cout<<path->x<<' '<<path->y<<endl;//nav_msgs::Path path_msg;if(path.empty()){cout<<"\033[1;31mFail generate path!\033[0m"<<endl;//ROS_INFO("\033[1;31mFail generate path!\033[0m");}for(auto coordinate=path.end()-1; coordinate>=path.begin(); --coordinate){// geometry_msgs::PoseStamped point_pose;// Remmaping coordinate//point_pose.pose.position.x = (coordinate->x + map_info_.origin.position.x / map_info_.resolution) * map_info_.resolution;//point_pose.pose.position.y = (coordinate->y + map_info_.origin.position.y / map_info_.resolution) * map_info_.resolution;//path_msg.poses.push_back(point_pose);cout<<coordinate->x<<" "<<coordinate->y<<endl;}//path_msg.header.frame_id = "map";// pub_robot_path_.publish(path_msg);//ROS_INFO("\033[1;36mSuccess generate path!\033[0m");// ros::spin();return 0;
}

完整工程参见https://github.com/JackJu-HIT/A-star/tree/master.

4.Reference

1. ROS-根据map.yaml进行像素坐标和map坐标的转换
2. ROS中map、costmap数据格式