当前位置: 首页 > news >正文

机器人控制算法—如何使用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
  1. ROS-根据map.yaml进行像素坐标和map坐标的转换
  2. ROS中map、costmap数据格式

相关文章:

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

1.Introduction 近期正在做全局规划局部动态规划的项目&#xff0c;目前遇到的问题是&#xff0c;我们如何利用C处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理&#xff0c;这样更好地在非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 时延相关 之前我们学习过&#xff0c;时延由发送时延、传播时延和处理时延三部分构成。 发送时延的计算公式为“分组长度除以发送速率”&#xff0c; 发送速率应该从网卡速率、信道带宽、以及对端的接口速率中取最小。 传播时延的计…...

CSS3媒体查询与页面自适应

2017年9月&#xff0c;W3C发布媒体查询(Media Query Level 4)候选推荐标准规范&#xff0c;它扩展了已经发布的媒体查询的功能。该规范用于CSS的media规则&#xff0c;可以为文档设定特定条件的样式&#xff0c;也可以用于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服务磁盘存储告急&#xff0c;最高可扩容16T。排查如下&#xff1a;主要是视频文件存在大量复制使用的情况。例如发布节目时复制、预览时复制&#xff0c;这样上传一份视频后最大会有四份拷贝&#xff08;预览、普通发布、互动…...

C++适配器

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

基于openresty waf二次开发多次匹配到的ip再做拉黑

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

新一代构建工具Vite-xyphf

一、什么vite? vite:是一款思维比较前卫而且先进的构建工具,他解决了一些webpack解决不了的问题——在开发环境下可以实现按需编译&#xff0c;加快了开发速度。而在生产环境下&#xff0c;它使用Rollup进行打包&#xff0c;提供更好的tree-shaking、代码压缩和性能优化&…...

Flink源码解析三之执行计划⽣成

JobManager Leader 选举 首先flink会依据配置获取RecoveryMode,RecoveryMode一共两两种:STANDALONE和ZOOKEEPER。 如果用户配置的是STANDALONE,会直接去配置中获取JobManager的地址如果用户配置的是ZOOKEEPER,flink会首先尝试连接zookeeper,利用zookeeper的leadder选举服务发现…...

Flutter 常见错误记录总结

1、当 flutter pub get 指令报如下错误时&#xff1a; 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

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

【Hydro】部分基流分割方法及程序代码说明

目录 说明一、数字滤波法单参数数字滤波Lyne-Hollick滤波法Chapman滤波法Chapman-Maxwell滤波法Boughton-Chapman滤波法 双参数滤波法Eckhardt滤波法 二、其他基流分割方法基流指数&#xff08;BFI&#xff09;法时间步长&#xff08;HYSEP&#xff09;法PART法加里宁-阿里巴扬…...

C#Regex正则表达式(Regular Expression)

在C#中&#xff0c;Regex是正则表达式&#xff08;Regular Expression&#xff09;的缩写&#xff0c;它是一种强大的文本匹配和处理工具。正则表达式是一种用于描述模式的字符串&#xff0c;它可以用来在文本中查找、替换和提取满足特定模式的内容。 在C#中&#xff0c;你可以…...

Wi-Fi还可以做什么?柯南解释IOT应用

大会报告&#xff1a;无线人工智能技术正在改变世界 Wi-Fi还可以做什么&#xff1f;随着带宽的提升&#xff0c;无线终端可以识别出更多的多径&#xff0c;每条多径都可以视作一个虚拟传感器&#xff0c;以感知周边环境。基于此&#xff0c;越来越多的无线感知产品应运而生。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的安装根目录&#xff1a;$HIVE_HOME/conf 创建一个新的配置文件&#xff1a;beeline-hs2-connection.xml <?xml version"1.0"?> <?xml-stylesheet type"text/xsl…...

[笔记] %的含义

取模 不赘述。 引导符 重点说一下在printf("%d", n);中的意思。 这里的意思是&#xff1a;将""外对应位置的结果返回给引导符所在的位置&#xff0c; %后面跟着的是结果对应的数据类型&#xff0c; 只有数据类型匹配才能正确输出结果。...

FRI及相关SNARKs的Fiat-Shamir安全

1. 引言 本文主要参考&#xff1a; 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案例学习&#xff1a;简单的音频识别 我们简单学习了音频识别。这次我们继续学习如何使用成熟的语音分类模型来进行迁移学习 官方教程&#xff1a; 使用 YAMNet 进行迁移学习&#xff0c;用于环境声音分类 模型下载地址&#xff08;需要科学上网&…...

IDEA运行Tomcat出现乱码问题解决汇总

最近正值期末周&#xff0c;有很多同学在写期末Java web作业时&#xff0c;运行tomcat出现乱码问题&#xff0c;经过多次解决与研究&#xff0c;我做了如下整理&#xff1a; 原因&#xff1a; IDEA本身编码与tomcat的编码与Windows编码不同导致&#xff0c;Windows 系统控制台…...

OpenLayers 可视化之热力图

注&#xff1a;当前使用的是 ol 5.3.0 版本&#xff0c;天地图使用的key请到天地图官网申请&#xff0c;并替换为自己的key 热力图&#xff08;Heatmap&#xff09;又叫热点图&#xff0c;是一种通过特殊高亮显示事物密度分布、变化趋势的数据可视化技术。采用颜色的深浅来显示…...

CTF show Web 红包题第六弹

提示 1.不是SQL注入 2.需要找关键源码 思路 进入页面发现是一个登录框&#xff0c;很难让人不联想到SQL注入&#xff0c;但提示都说了不是SQL注入&#xff0c;所以就不往这方面想了 ​ 先查看一下网页源码&#xff0c;发现一段JavaScript代码&#xff0c;有一个关键类ctfs…...

k8s从入门到放弃之Ingress七层负载

k8s从入门到放弃之Ingress七层负载 在Kubernetes&#xff08;简称K8s&#xff09;中&#xff0c;Ingress是一个API对象&#xff0c;它允许你定义如何从集群外部访问集群内部的服务。Ingress可以提供负载均衡、SSL终结和基于名称的虚拟主机等功能。通过Ingress&#xff0c;你可…...

阿里云ACP云计算备考笔记 (5)——弹性伸缩

目录 第一章 概述 第二章 弹性伸缩简介 1、弹性伸缩 2、垂直伸缩 3、优势 4、应用场景 ① 无规律的业务量波动 ② 有规律的业务量波动 ③ 无明显业务量波动 ④ 混合型业务 ⑤ 消息通知 ⑥ 生命周期挂钩 ⑦ 自定义方式 ⑧ 滚的升级 5、使用限制 第三章 主要定义 …...

渗透实战PortSwigger靶场-XSS Lab 14:大多数标签和属性被阻止

<script>标签被拦截 我们需要把全部可用的 tag 和 event 进行暴力破解 XSS cheat sheet&#xff1a; https://portswigger.net/web-security/cross-site-scripting/cheat-sheet 通过爆破发现body可以用 再把全部 events 放进去爆破 这些 event 全部可用 <body onres…...

什么是EULA和DPA

文章目录 EULA&#xff08;End User License Agreement&#xff09;DPA&#xff08;Data Protection Agreement&#xff09;一、定义与背景二、核心内容三、法律效力与责任四、实际应用与意义 EULA&#xff08;End User License Agreement&#xff09; 定义&#xff1a; EULA即…...

关于 WASM:1. WASM 基础原理

一、WASM 简介 1.1 WebAssembly 是什么&#xff1f; WebAssembly&#xff08;WASM&#xff09; 是一种能在现代浏览器中高效运行的二进制指令格式&#xff0c;它不是传统的编程语言&#xff0c;而是一种 低级字节码格式&#xff0c;可由高级语言&#xff08;如 C、C、Rust&am…...

[Java恶补day16] 238.除自身以外数组的乘积

给你一个整数数组 nums&#xff0c;返回 数组 answer &#xff0c;其中 answer[i] 等于 nums 中除 nums[i] 之外其余各元素的乘积 。 题目数据 保证 数组 nums之中任意元素的全部前缀元素和后缀的乘积都在 32 位 整数范围内。 请 不要使用除法&#xff0c;且在 O(n) 时间复杂度…...

基于Java+MySQL实现(GUI)客户管理系统

客户资料管理系统的设计与实现 第一章 需求分析 1.1 需求总体介绍 本项目为了方便维护客户信息为了方便维护客户信息&#xff0c;对客户进行统一管理&#xff0c;可以把所有客户信息录入系统&#xff0c;进行维护和统计功能。可通过文件的方式保存相关录入数据&#xff0c;对…...