机器人控制算法—如何使用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 #用率小于此阈值的像素被认为是完全空闲的
需要注意的是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 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为分辨率,则:
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
- ROS-根据map.yaml进行像素坐标和map坐标的转换
- ROS中map、costmap数据格式
相关文章:
机器人控制算法—如何使用C++读取pgm格式的栅格地图并转化为ROS地图格式的data?
1.Introduction 近期正在做全局规划局部动态规划的项目,目前遇到的问题是,我们如何利用C处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理,这样更好地在非ROS平台下移植。 2.Main 如下几条信息…...

牛客项目(五)-使用kafka实现发送系统通知
kafka入门以及与spring整合 Message.java import java.util.Date;public class Message {private int id;private int fromId;private int toId;private String conversationId;private String content;private int status;private Date createTime;public int getId() {retur…...
计算机网络——第一章时延部分深入学习、相关习题及详细解析
目录 时延相关 习题1 习题1-改 习题2 时延相关 之前我们学习过,时延由发送时延、传播时延和处理时延三部分构成。 发送时延的计算公式为“分组长度除以发送速率”, 发送速率应该从网卡速率、信道带宽、以及对端的接口速率中取最小。 传播时延的计…...

CSS3媒体查询与页面自适应
2017年9月,W3C发布媒体查询(Media Query Level 4)候选推荐标准规范,它扩展了已经发布的媒体查询的功能。该规范用于CSS的media规则,可以为文档设定特定条件的样式,也可以用于HTML、JavaScript等语言。 1、媒体查询基础 媒体查询…...

UG\NX二次开发 超长的对象属性值,怎么设置
文章作者:里海 来源网站:里海NX二次开发3000例专栏 感谢粉丝订阅 感谢 Dr. Lin 订阅本专栏,非常感谢。 简介 使用UF_ATTR_assign设置对象属性,如果属性值超过UF_ATTR_MAX_STRING_LEN则会报错。 #define UF_ATTR_MAX_STRING_LEN 132 怎么办呢?下面这种方法可以解决: 效果 …...

流媒体服务实现H5实时预览视频
目录 背景方案业务实践细节注意 待办 背景 客户aws服务磁盘存储告急,最高可扩容16T。排查如下:主要是视频文件存在大量复制使用的情况。例如发布节目时复制、预览时复制,这样上传一份视频后最大会有四份拷贝(预览、普通发布、互动…...

C++适配器
文章目录 引言栈和队列 priority_queue仿函数迭代器区间 引言 栈的特性是先进后出,队列的特性是先进先出,然而双向队列同时具有栈和队列的特性,所以我们可以通过双向队列来适配出栈和队列。 先看库里面 栈和队列 stack和queue模板参数里面都…...

基于openresty waf二次开发多次匹配到的ip再做拉黑
我们想在openresty waf的基础上做二次开发,比如再精确一些。比如我们先匹配到了select的url我们先打分10分,匹配到cc 1000/s我们再给这个ip打10分…直到100分我们就拉黑这个ip。 [openresty waf][1] #cat reids_w.lua require lib local redis require…...

新一代构建工具Vite-xyphf
一、什么vite? vite:是一款思维比较前卫而且先进的构建工具,他解决了一些webpack解决不了的问题——在开发环境下可以实现按需编译,加快了开发速度。而在生产环境下,它使用Rollup进行打包,提供更好的tree-shaking、代码压缩和性能优化&…...
Flink源码解析三之执行计划⽣成
JobManager Leader 选举 首先flink会依据配置获取RecoveryMode,RecoveryMode一共两两种:STANDALONE和ZOOKEEPER。 如果用户配置的是STANDALONE,会直接去配置中获取JobManager的地址如果用户配置的是ZOOKEEPER,flink会首先尝试连接zookeeper,利用zookeeper的leadder选举服务发现…...
Flutter 常见错误记录总结
1、当 flutter pub get 指令报如下错误时: pub get failed command: "/Users/***/developer/flutter/bin/cache/dart-sdk/bin/dart __deprecated_pub --color --directory . get --example" pub env: { "FLUTTER_ROOT": "/Users/***/dev…...

[ASP]校无忧在线报名系统 v2.1
校无忧在线报名系统为了满足各地不同的报名人员的需求,为提供更为高效、方便、快捷的报名条件,同时也为减轻管理人员的工作难度;更为协调报名人员与管理人员的关系,快速提高了报名人员与管理人员的工作效率应运而生。系统适用于政…...

【Hydro】部分基流分割方法及程序代码说明
目录 说明一、数字滤波法单参数数字滤波Lyne-Hollick滤波法Chapman滤波法Chapman-Maxwell滤波法Boughton-Chapman滤波法 双参数滤波法Eckhardt滤波法 二、其他基流分割方法基流指数(BFI)法时间步长(HYSEP)法PART法加里宁-阿里巴扬…...
C#Regex正则表达式(Regular Expression)
在C#中,Regex是正则表达式(Regular Expression)的缩写,它是一种强大的文本匹配和处理工具。正则表达式是一种用于描述模式的字符串,它可以用来在文本中查找、替换和提取满足特定模式的内容。 在C#中,你可以…...

Wi-Fi还可以做什么?柯南解释IOT应用
大会报告:无线人工智能技术正在改变世界 Wi-Fi还可以做什么?随着带宽的提升,无线终端可以识别出更多的多径,每条多径都可以视作一个虚拟传感器,以感知周边环境。基于此,越来越多的无线感知产品应运而生。20…...
centos部署java程序
后台启动java程序 nohup java -jar -XX:HeapDumpOnOutOfMemoryError -XX:HeapDumpPath/data/app1/logs/ /data/app1.jar --spring.config.location/data/app1/config/application.properties,/data/app1/config/application-dev.properties > /data/app1/logs 2>&1…...
Sqoop导入到Hive,Hive使用 HA
Sqoop写入Hive卡在连接Hive的JDBC上不执行 Sqoop访问 启用 HA模式的Hive 找到Hive的安装根目录:$HIVE_HOME/conf 创建一个新的配置文件:beeline-hs2-connection.xml <?xml version"1.0"?> <?xml-stylesheet type"text/xsl…...
[笔记] %的含义
取模 不赘述。 引导符 重点说一下在printf("%d", n);中的意思。 这里的意思是:将""外对应位置的结果返回给引导符所在的位置, %后面跟着的是结果对应的数据类型, 只有数据类型匹配才能正确输出结果。...

FRI及相关SNARKs的Fiat-Shamir安全
1. 引言 本文主要参考: Alexander R. Block 2023年论文 Fiat-Shamir Security of FRI and Related SNARKsAlbert Garreta 2023年9月在ZK Summit 10上分享 ZK10: Fiat-Shamir security of FRI and related SNARKs - Albert Garreta (Nethermind) 评估参数用的Sage…...

TensorFlow案例学习:使用 YAMNet 进行迁移学习,对音频进行识别
前言 上一篇文章 TensorFlow案例学习:简单的音频识别 我们简单学习了音频识别。这次我们继续学习如何使用成熟的语音分类模型来进行迁移学习 官方教程: 使用 YAMNet 进行迁移学习,用于环境声音分类 模型下载地址(需要科学上网&…...

Qt/C++开发监控GB28181系统/取流协议/同时支持udp/tcp被动/tcp主动
一、前言说明 在2011版本的gb28181协议中,拉取视频流只要求udp方式,从2016开始要求新增支持tcp被动和tcp主动两种方式,udp理论上会丢包的,所以实际使用过程可能会出现画面花屏的情况,而tcp肯定不丢包,起码…...

【JavaEE】-- HTTP
1. HTTP是什么? HTTP(全称为"超文本传输协议")是一种应用非常广泛的应用层协议,HTTP是基于TCP协议的一种应用层协议。 应用层协议:是计算机网络协议栈中最高层的协议,它定义了运行在不同主机上…...
系统设计 --- MongoDB亿级数据查询优化策略
系统设计 --- MongoDB亿级数据查询分表策略 背景Solution --- 分表 背景 使用audit log实现Audi Trail功能 Audit Trail范围: 六个月数据量: 每秒5-7条audi log,共计7千万 – 1亿条数据需要实现全文检索按照时间倒序因为license问题,不能使用ELK只能使用…...

cf2117E
原题链接:https://codeforces.com/contest/2117/problem/E 题目背景: 给定两个数组a,b,可以执行多次以下操作:选择 i (1 < i < n - 1),并设置 或,也可以在执行上述操作前执行一次删除任意 和 。求…...
Nginx server_name 配置说明
Nginx 是一个高性能的反向代理和负载均衡服务器,其核心配置之一是 server 块中的 server_name 指令。server_name 决定了 Nginx 如何根据客户端请求的 Host 头匹配对应的虚拟主机(Virtual Host)。 1. 简介 Nginx 使用 server_name 指令来确定…...
Caliper 配置文件解析:config.yaml
Caliper 是一个区块链性能基准测试工具,用于评估不同区块链平台的性能。下面我将详细解释你提供的 fisco-bcos.json 文件结构,并说明它与 config.yaml 文件的关系。 fisco-bcos.json 文件解析 这个文件是针对 FISCO-BCOS 区块链网络的 Caliper 配置文件,主要包含以下几个部…...

【MATLAB代码】基于最大相关熵准则(MCC)的三维鲁棒卡尔曼滤波算法(MCC-KF),附源代码|订阅专栏后可直接查看
文章所述的代码实现了基于最大相关熵准则(MCC)的三维鲁棒卡尔曼滤波算法(MCC-KF),针对传感器观测数据中存在的脉冲型异常噪声问题,通过非线性加权机制提升滤波器的抗干扰能力。代码通过对比传统KF与MCC-KF在含异常值场景下的表现,验证了后者在状态估计鲁棒性方面的显著优…...
苹果AI眼镜:从“工具”到“社交姿态”的范式革命——重新定义AI交互入口的未来机会
在2025年的AI硬件浪潮中,苹果AI眼镜(Apple Glasses)正在引发一场关于“人机交互形态”的深度思考。它并非简单地替代AirPods或Apple Watch,而是开辟了一个全新的、日常可接受的AI入口。其核心价值不在于功能的堆叠,而在于如何通过形态设计打破社交壁垒,成为用户“全天佩戴…...
从面试角度回答Android中ContentProvider启动原理
Android中ContentProvider原理的面试角度解析,分为已启动和未启动两种场景: 一、ContentProvider已启动的情况 1. 核心流程 触发条件:当其他组件(如Activity、Service)通过ContentR…...
小木的算法日记-多叉树的递归/层序遍历
🌲 从二叉树到森林:一文彻底搞懂多叉树遍历的艺术 🚀 引言 你好,未来的算法大神! 在数据结构的世界里,“树”无疑是最核心、最迷人的概念之一。我们中的大多数人都是从 二叉树 开始入门的,它…...