1.Introduction
近期正在做全局规划+局部动态规划的项目，目前遇到的问题是，我们如何利用C++处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理，这样更好地在非ROS平台下移植。
2.Main
如下几条信息需要了解：
（1）data[]是按照那张地图图片的自底向上，自左至右逐个像素点存储的.
(2) 在使用二维地图定位导航时，建好的地图文件中包括 m a p . p g m map.pgm map.pgm和 m a p . y a m l map.yaml map.yaml.其中.yaml文件如下:
bash
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[]复原成地图图片像素坐标关系为：
cpp
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 w_x w_y wxwy表示地图坐标系下的坐标，resolution为分辨率,则:
cpp
image_x = (wx - origin_x) / resolution
image_y = (wy - origin_y) / resolution（5）由图像像素坐标->地图坐标
image_x,image_y表示在图像像素坐标系中的坐标
w x w y w_x w_y wxwy表示地图坐标系下的坐标，resolution为分辨率,则：
cpp
wx=image_x*resolution+origin_x
wy=image_y*resolution+origin_y3.Examples
我们举了一个从地图pgm读取到处理成目标地图数据格式data[] 的例子。
cpp
int main(int argc, char **argv)
{
PathGenerator PG;
//Read pgm
cv::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 coordinate
float 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, Options
PG.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 Wall
int 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 coordinate
AStar::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 Path
auto 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.