机器人控制算法—如何使用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 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(r, c);
        }
    }
    cout<<"0"<
     // 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> 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"<

    for(int i = 0;i<data.size();i++){
        cout<<data[i]<<" ";
    }
    cout<<endl;
    
    //cout<<"maze.size()="<
    //cout<<"maze[0].size()"<
	//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<x<<' '<y<
     
    //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数据格式