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


    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文件如下:

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

    需要注意的是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;
        }   
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11

    像素坐标wx,wy复原成data索引如下:

    /**
       * @brief  Given two map coordinates... compute the associated index
       * @param mx The x coordinate
       * @param my The y coordinate
       * @return The associated index
       */
      inline unsigned int getIndex(unsigned int mx, unsigned int my) const
      {
        return my * size_x_ + mx;
      }
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

    重点是my * size_x_ + mx;把二维数映射成成一维数组索引。 s i z e size size_ x 代表地图宽度( y ) x代表地图宽度(y) x代表地图宽度(y

    (4) 由地图坐标->图像像素坐标
    基于地图的坐标转换到图像坐标系上
    w x w y w_x w_y wxwy表示地图坐标系下的坐标,resolution为分辨率,则:

    image_x = (wx - origin_x) / resolution
    image_y = (wy - origin_y) / resolution
    
    
    • 1
    • 2
    • 3

    (5)由图像像素坐标->地图坐标
    image_x,image_y表示在图像像素坐标系中的坐标
    w x w y w_x w_y wxwy表示地图坐标系下的坐标,resolution为分辨率,则:

    wx=image_x*resolution+origin_x
    wy=image_y*resolution+origin_y
    
    • 1
    • 2
    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;
    }
    
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161

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

    ------add 20231212
    如下是一个模拟地图读取pgm过程,并解析成地图坐标data,再将其还原成图片显示的过程。

        //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;
       
       cv::imshow("oriinal", m4); //原始地图图片,原点在左上角,像素坐标系
       //cv::waitKey(0);
    
       vector<char> data;
        for(int i = m4.rows-1;i >= 0;i--){
    		for(int j = 0;j < m4.cols;j++){
                {
                    data.push_back(m4.at<unsigned char>(i,j));  //处理成data[]一维数组
    		    }
            }
    	}
        
         cv::Mat map_info( m4.cols,  m4.rows, CV_8UC1);//initlize
         cv::Mat map_info_Rotate( m4.cols,  m4.rows, CV_8UC1);//initlize
        for(int i = (m4.cols)*(m4.rows) - 1; i > 0; i--)
        {
            int x = i%m4.cols;  //还原为像素坐标
            int y = i/m4.cols;  //还原为像素坐标        // 还原成二维数组,但原点在左下角
            //unsigned char value = 255;
            map_info.at<unsigned char>(x, y) = data[i];
            //cout<
        }
    
        //
        int l = 0;int k = m4.rows;
        for(int j = 0,k = m4.rows; j < m4.rows; j++,k--){
           for(int i = 0,l = 0; i < m4.cols;i++,l++){
               map_info_Rotate.at<unsigned char>(l,k) = map_info.at<unsigned char>(i, j);
           }
        }
        //如果显示,需要将坐标系原点从左下角转换成左上角。
        cv::imshow("map_info", map_info);       
        cv::rotate(map_info, map_info_Rotate,cv::ROTATE_90_COUNTERCLOCKWISE);// cv::ROTATE_90_CLOCKWISE);
        cv::imshow("map_info_Rotate", map_info_Rotate);
    
        cv::waitKey(0);
    
       
       return 0;
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
       //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;
       
       cv::imshow("oriinal", m4);
       //cv::waitKey(0);
    
       vector<char> data;
        for(int i = m4.rows-1;i >= 0;i--){
    		for(int j = 0;j < m4.cols;j++){
                {
                    data.push_back(255 - m4.at<unsigned char>(i,j));
    		    }
            }
    	}
        
         cv::Mat map_info( m4.cols,  m4.rows, CV_8UC1);//initlize
         cv::Mat map_info_Rotate( m4.cols,  m4.rows, CV_8UC1);//initlize
        for(int i = (m4.cols)*(m4.rows) - 1; i > 0; i--)
        {
            int x = i%m4.cols;  //还原为像素坐标
            int y = i/m4.cols;  //还原为像素坐标
            //unsigned char value = 255;
            map_info.at<unsigned char>(x, y) =  data[i];
            //cout<
        }
    
        //
        int l = 0;int k = m4.rows;
        for(int j = 0,k = m4.rows; j < m4.rows; j++,k--){
           for(int i = 0,l = 0; i < m4.cols;i++,l++){
               map_info_Rotate.at<unsigned char>(l,k) = map_info.at<unsigned char>(i, j);
           }
        }
        cv::imshow("map_info", map_info);
        cv::rotate(map_info, map_info_Rotate,cv::ROTATE_90_COUNTERCLOCKWISE);// cv::ROTATE_90_CLOCKWISE);
        cv::imshow("map_info_Rotate", map_info_Rotate);
    
        cv::waitKey(0);
    
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41

    在这里插入图片描述

    4.Reference
    1. ROS-根据map.yaml进行像素坐标和map坐标的转换
    2. ROS中map、costmap数据格式
    3. SLAM2d栅格地图构建的常用方法
  • 相关阅读:
    Spring Data Web支持
    CTFHUB前置知识——http协议
    Service和启动其他应用的功能和替换Fragment
    国际盲人节|爱尔眼科“医”心帮“盲”,“眶”护光明
    webpack 配置
    vue3+ts项目04-国际化
    vue2脚手架组件化开发
    使用Transient noise和ac noise仿真晶体管噪声
    Spring源码(一)IOC之ClassPathXmlApplicationContext
    List append 和 += 的区别
  • 原文地址:https://blog.csdn.net/qq_40464599/article/details/134174046