网站关键词优化怎么做的,湖南住房和城乡建设网站,wordpress新闻动态不显示作者,邢台信息港房产出租1.Introduction
近期正在做全局规划局部动态规划的项目#xff0c;目前遇到的问题是#xff0c;我们如何利用C处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理#xff0c;这样更好地在非ROS平台下移植。
2.Main
如下几条信息…1.Introduction
近期正在做全局规划局部动态规划的项目目前遇到的问题是我们如何利用C处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理这样更好地在非ROS平台下移植。
2.Main
如下几条信息需要了解 1data[]是按照那张地图图片的自底向上自左至右逐个像素点存储的. (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 #用率小于此阈值的像素被认为是完全空闲的需要注意的是origin: [-49.0286, -107.401, 0.0] #图像左下角在地图坐标下的坐标我们后续利用这条信息建立像素与世界坐标系之间的关系。 (3)实际上我们在路径规划实施过程中是接收到地图像素信息data[],(一维数组)然后将其复原为原来的像素坐标再进行路径规划处理。 data[]复原成地图图片像素坐标关系为 for(int i 0; imap_info_width*map_info_height; i){x i%map_info_width; //还原为像素坐标y i/map_info_width; //还原为像素坐标if(data[i] ! 0){ coutobstacle:endl;//PG.map_generator_.addCollision({x, y}, 3);PG.map_generator_.addCollision({x, y}, 3);}coutendl;} (4) 由地图坐标-图像像素坐标 基于地图的坐标转换到图像坐标系上 w x w y w_x w_y wxwy表示地图坐标系下的坐标resolution为分辨率,则:
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为分辨率,则
wximage_x*resolutionorigin_x
wyimage_y*resolutionorigin_y3.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.atunsigned char(r, c);}}cout0endl;*/// 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;/*vectorvectorint 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 }};*/vectorvectorint 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 }};vectorint data;for(int i m4.rows-1;i 0;i--){for(int j 0;j m4.cols;j){if(m4.atunsigned 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);}}}coutcishunumendl;*/for(int i 0;idata.size();i){coutdata[i] ;}coutendl;//coutmaze.size()maze.size()endl;//coutmaze[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-3endl;// Add Wallint x, y;for(int i 0; imap_info_width*map_info_height; i){x i%map_info_width;y i/map_info_width;coutiiendl;coutsum:map_info_width*map_info_heightendl;double v double(i/(map_info_width*map_info_height));coutv%endl;if(data[i] ! 0){ coutobstacle:endl;//PG.map_generator_.addCollision({x, y}, 3);PG.map_generator_.addCollision({x, y}, 3);cout(x,y) ;}coutendl;} cout-2endl; // 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;cout1endl;// Find Pathauto path PG.map_generator_.findPath(source, target);cout2endl;//coutpath-x path-yendl;//nav_msgs::Path path_msg;if(path.empty()){cout\033[1;31mFail generate path!\033[0mendl;//ROS_INFO(\033[1;31mFail generate path!\033[0m);}for(auto coordinatepath.end()-1; coordinatepath.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);coutcoordinate-x coordinate-yendl;}//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
ROS-根据map.yaml进行像素坐标和map坐标的转换ROS中map、costmap数据格式
