46.在ROS中实现global planner(2)
前文实现了一个global planner的模板,并且可以工作,本文将实现astar算法,为后续完成一个astar global planner做准备
1. AStar简介
1.1 AStar
Astar算法是一种图形搜索算法,常用于寻路。Astar算法原理网上可以找到很多,简单的说就是,从起点开始,向外发散,再去其中每个点到终端的估计距离最短的,继续循环上次步骤,直到到达目标点。
1.2 启发函数
估算距离(f)=距离起点距离(G)+距离终点的距离(H)
显然G是已知的,
- 第一次从起点开始,G当然是0,
- 向外发散也就是上下左右,距离起点当然是1,也就是其父节点的G+1
H 是距离目标点的距离,我们就是要规划路径,怎么找到距离目标有多远,其实这个距离是估计理想距离,当没有障碍物的时的距离,也就是直线距离
这里的直线距离又有两种方式表示
- 曼哈顿距离
x+yx+y x+y - 欧式距离
x2+y2\sqrt{x^2 + y^2} x2+y2
显然网格计算适合使用曼哈顿距离的,其计算消耗要小很多
2. 实现过程
2.1 数据结构
上面简单提到实现过程,下面我们先定义数据结构, 我们需要保存当前已经搜索的节点,同时需要找到最小的f值,然后在该节点进行继续搜索和添加
- 节点定义
class Grid {public:Point parent_point_;Point point_;float g_;float h_; // f = g + h
};
节点定义比较简单,也就是当前点坐标,父节点坐标,g,h值
- open list
需要保存当前已经搜索点的列表,由于下次搜索有需要搜有f最小值,我们定义一个有限队列,这样我们取top就可以得到最小f的节点
struct greater {
bool operator()(const Grid& g1, const Grid& g2) const {float f1 = g1.h_ + g1.g_;float f2 = g2.h_ + g2.g_;return f1 > f2 || (f1 == f2 && g1.g_ < g2.g_);
}
};
std::priority_queue<Grid, std::vector<Grid>, greater> open_list_;
2.2 邻域
邻域定义较简单,定义为相对该点的偏移即可
std::vector<Point> neighbors_;// 四邻域neighbors_.emplace_back(-1, 0);neighbors_.emplace_back(1, 0);neighbors_.emplace_back(0, -1);neighbors_.emplace_back(0, 1);// 八领域再加上下面neighbors_.emplace_back(-1, -1);neighbors_.emplace_back(1, 1);neighbors_.emplace_back(1, -1);neighbors_.emplace_back(-1, 1);
2.3 搜索实现
2.3.1 搜索过程
简单概括就是搜索过程就是不断最小的f值的节点的邻域,直到到达终点
伪代码如下
open_list.push(start);while(!open_list_.empty()) {// 取最前面的也就是最小的f节点Grid grid = open_list.top();open_list.pop();// 直到当前搜索点 为终点,终止循环if (grid.point == end.point) {return true;}// 循环这个节点的邻居V节点, 分别计算g h, 同时把这些节点添加到open_listfor (neighbor:neighbors) {Grid current;current.g_ = grid.g_ + 1current.h_ = calc_h(grid, neighbor, end); // 计算邻域的hcurrent.parent_point_ = grid.point; // 更新父节点if (!(current in open_list)) {// 如果该点已经不在open list中则添加open_list.push(current);else {// 如果该点已经存在open list中 则根据V计算结果确认是否需要更新float f = current.g_ + current.h_;open_list[current.point].g_ = current.g_ ;open_list[current.point].h_ = current.h_ ;open_list[current.point].parent_point_ = grid.point; // 更新父节点}}
}
2.3.2 得到路径
grid结构可以看出来,其实相当于一个链表结构,找到路径后,只需要从end循环即可得到路径
bool GetPathFromGrid(const Point& start_point, const Point& end_point, std::vector<Point>& path) {path.clear();path.push_back(end_point);int start_index;bool ret = Point2Index(start_point, start_index);if (!ret) {return false;}int index;Point point = end_point;ret = Point2Index(point, index);if (!ret) {return false;}while (index != start_index) {point = all_grid_[index].parent_point_;path.push_back(point);Point2Index(point, index);}return true;
}
3. 测试验证
3.1 输入
为了方便我们直接读取png图,这样我们直接编辑图就可以直接用于测试,
// 使用opencv直接读取png图片cv::Mat mat = cv::imread("../map/map_demo.png", cv::IMREAD_GRAYSCALE);// 为了保持习惯 我们反转下, 值255认为障碍物(读取的图片255是白色)cv::threshold(mat, mat, 128, 255, CV_THRESH_BINARY_INV);
3.2 显示
为了方便我们观察过程,我们设计一个函数用于显示规划和过程,为了简便我们使用opencv窗口
void Display(const cv::Mat& map_data, // 传入grid mapcv::Point begin, // 起点cv::Point end, // 终点const std::vector<cv::Point>& path, // 输出的路径const std::vector<cv::Point>& close_list // 已经完成搜索的点);
4. 测试
-
输入地图

-
测试结果

相关文章:
46.在ROS中实现global planner(2)
前文实现了一个global planner的模板,并且可以工作,本文将实现astar算法,为后续完成一个astar global planner做准备 1. AStar简介 1.1 AStar Astar算法是一种图形搜索算法,常用于寻路。Astar算法原理网上可以找到很多,简单的说…...
05- 泰坦尼克号海难生死预测 (机器学习集成算法) (项目五)
Kaggle: 一个数据建模和数据分析竞赛平台sns画柱状图: sns.barplot(datatrain,xPclass,ySurvived)查看数据分布(survived 和 fare): sns.FacetGrid(train,hueSurvived,aspect3) ageFacetsns.FacetGrid(train,hueSurvived,aspect3) ageFacet.map(sns.kdeplot,Fare,shadeTrue) …...
【python百炼成魔】python运算符的使用与输入输出函数
文章目录前言一. python 运算符1.1 算术运算符1.2 .赋值运算符1.3 比较运算符1.4. 布尔运算符二. 输入和输出函数2.1 print函数2.1.1 help函数查看帮助文档2.1.2 print的格式化输出2.2 format函数2.3 input数据接收函数写在最后前言 Python 中的运算符主要分为算术运算符、比较…...
uniapp实现app检查更新与升级-uni-upgrade-center详解
app检查更新与升级 参考链接: 升级中心uni-upgrade-center - App uni-admin h5 api App资源在线升级更新 uni-app使用plus注意事项 关于在线升级(WGT)的几个疑问 什么是升级中心uni-upgrade-center uniapp官方开发的App版本更新的插件&#…...
公司项目引入这种方式,开发应用真是又快又准!
试想一下,你开足马力提了一串需求,给开发精英团队也好,给外包也行,都要等个半年甚至更久才会给到你一个满意的产品,你是否还有动力? 这还不止,业务越来越复杂,最初的需求也在随着着…...
virtuoso数据库介绍
在国内,对海量 RDF 数据的管理有着迫切的实际需求; RDF:Resource Description Framework,是一个使用XML语法来表示的资料模型(Data model),用来描述Web资源的特性,及资源与资源之间的关系。 Virtuoso可以对…...
linux高级命令之编辑器 vim
编辑器 vim学习目标能够说出vim的三种工作模式能够说出vim对应复制和粘贴命令1. vim 的介绍vim 是一款功能强大的文本编辑器,也是早年 Vi 编辑器的加强版,它的最大特色就是使用命令进行编辑,完全脱离了鼠标的操作。2. vim 的工作模式命令模式…...
分布式光伏储能系统的优化配置方法(Matlab代码实现)
💥💥💞💞欢迎来到本博客❤️❤️💥💥 🏆博主优势:🌞🌞🌞博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️座右铭&a…...
Grafana loki部署及使用及问题处理方法(超详细)
一、下载软件 因为我是本地测试,所以用的windows版本的包,loki服务window版本的安装包下载地址:下载地址,选择 promtail-windows版本的安装包下载地址:下载地址 Grafana服务的下载地址:下载地址 二、配置…...
vue项目如何使用 SheetJS(xlsx)插件?
简言 SheetJS是一款非常好用的前端处理表格文件的工具。它分社区版和专业版,我们今天来介绍如何简单使用它的社区版。 SheetJS社区版官网 介绍 你应该打开官网浏览具体使用详情。 安装 打开官网在如上图的Installation板块中可以找到各种运行模块的使用方式。 …...
项目管理工具dhtmlxGantt甘特图入门教程(九):支持哪些数据格式(上篇)
dhtmlxGantt是用于跨浏览器和跨平台应用程序的功能齐全的Gantt图表,可满足项目管理控件应用程序的所有需求,是最完善的甘特图图表库这篇文章给大家讲解 dhtmlxGantt 的数据属性和数据库结构。 DhtmlxGantt正版试用下载(qun:764…...
iView Table合并单元格(行、列)
行/列合并设置属性 span-method 可以指定合并行或列的算法。该方法参数为 4 个对象:row: 当前行column: 当前列rowIndex: 当前行索引columnIndex: 当前列索引该函数可以返回一个包含两个元素的数组,第一个元素代表 rowspan,第二个元素代表 co…...
如何用P6软件编制项目进度计划(下)
卷首语 根据项目合同包含的工作范围进行工作分解(WBS),按照业主的要求及项目管理的需要,考虑不同阶段和层次,适时编制出项目管理所要求的的各级进度计划。 4搜集项目计划与进度控制相关信息 搜集与项目计划编制与进…...
环境配置完整指导——Installing C++ Distributions of PyTorch
目录一、前言二、动手开始做1. 安装cuda 11.42. 安装visual studio 2019 community3. 安装libtorch4. 安装mingw-w645. 配置环境变量6. 打开vscode开始写程序7. 运行程序8. 其他报错信息文章简介:这篇文章用于介绍在windows10 vscode中,跑通如下代码的全…...
深度学习——自注意力机制和位置编码(笔记)
1.自注意力: ①在深度学习中,经常使用卷积神经网络或者循环神经网络对序列进行编码 ②对于key,value和query,自注意力有一套自己的选法,因为key,value和query的值来自同一组输入。因此被称为自注意力或内部注意力 2…...
内网渗透(三十)之横向移动篇-利用远控工具向日葵横向移动
系列文章第一章节之基础知识篇 内网渗透(一)之基础知识-内网渗透介绍和概述 内网渗透(二)之基础知识-工作组介绍 内网渗透(三)之基础知识-域环境的介绍和优点 内网渗透(四)之基础知识-搭建域环境 内网渗透(五)之基础知识-Active Directory活动目录介绍和使用 内网渗透(六)之基…...
自动化测试中,该如何高效管理测试数据?
今晚在某个测试群,看到有人问了一个问题:把测试数据放配置文件读取和放文件通过函数调用读取有什么区别? 当时我下意识的这么回答:数据量越大,配置文件越臃肿,放在专门的数据文件(比如excel&am…...
Qt中项目A调用另一个项目B的方法汇总
在开发一个软件项目时候,当涉及到一个模块,已经有过类似的项目开发,为了避免重复开发,涉及到在该项目的工程中调用已开发的项目作为子项目,有很多种方法。 一、将项目编译成库文件然后进行调用 调用库文件通常有两种…...
【项目精选】基于Javaee的影视创作论坛的设计与实现(视频+论文+源码)
点击下载源码 基于Javaee的影视创作论坛的设计与实现主要用功能包括: 首页推荐、用户管理、影片管理、评论管理、 预告片管理、海报管理、公告管理、数据检索、用户注册与登录等等功能、统结构如下 (1)后台管理: 管理模块:管理员…...
深入【虚拟列表】动态高度、缓冲、异步加载... Vue实现
前言🎀 在前文中我们了解到: 1.在某种特殊场景下,我们需要将 大量数据 使用不分页的方式渲染到列表上,这种列表叫做长列表。 2.因为事件循环的机制,一次性大量的渲染耗时较长,并且渲染期间会阻塞页面交互…...
【Python】 -- 趣味代码 - 小恐龙游戏
文章目录 文章目录 00 小恐龙游戏程序设计框架代码结构和功能游戏流程总结01 小恐龙游戏程序设计02 百度网盘地址00 小恐龙游戏程序设计框架 这段代码是一个基于 Pygame 的简易跑酷游戏的完整实现,玩家控制一个角色(龙)躲避障碍物(仙人掌和乌鸦)。以下是代码的详细介绍:…...
条件运算符
C中的三目运算符(也称条件运算符,英文:ternary operator)是一种简洁的条件选择语句,语法如下: 条件表达式 ? 表达式1 : 表达式2• 如果“条件表达式”为true,则整个表达式的结果为“表达式1”…...
Java-41 深入浅出 Spring - 声明式事务的支持 事务配置 XML模式 XML+注解模式
点一下关注吧!!!非常感谢!!持续更新!!! 🚀 AI篇持续更新中!(长期更新) 目前2025年06月05日更新到: AI炼丹日志-28 - Aud…...
sqlserver 根据指定字符 解析拼接字符串
DECLARE LotNo NVARCHAR(50)A,B,C DECLARE xml XML ( SELECT <x> REPLACE(LotNo, ,, </x><x>) </x> ) DECLARE ErrorCode NVARCHAR(50) -- 提取 XML 中的值 SELECT value x.value(., VARCHAR(MAX))…...
新能源汽车智慧充电桩管理方案:新能源充电桩散热问题及消防安全监管方案
随着新能源汽车的快速普及,充电桩作为核心配套设施,其安全性与可靠性备受关注。然而,在高温、高负荷运行环境下,充电桩的散热问题与消防安全隐患日益凸显,成为制约行业发展的关键瓶颈。 如何通过智慧化管理手段优化散…...
linux 下常用变更-8
1、删除普通用户 查询用户初始UID和GIDls -l /home/ ###家目录中查看UID cat /etc/group ###此文件查看GID删除用户1.编辑文件 /etc/passwd 找到对应的行,YW343:x:0:0::/home/YW343:/bin/bash 2.将标红的位置修改为用户对应初始UID和GID: YW3…...
[Java恶补day16] 238.除自身以外数组的乘积
给你一个整数数组 nums,返回 数组 answer ,其中 answer[i] 等于 nums 中除 nums[i] 之外其余各元素的乘积 。 题目数据 保证 数组 nums之中任意元素的全部前缀元素和后缀的乘积都在 32 位 整数范围内。 请 不要使用除法,且在 O(n) 时间复杂度…...
Android 之 kotlin 语言学习笔记三(Kotlin-Java 互操作)
参考官方文档:https://developer.android.google.cn/kotlin/interop?hlzh-cn 一、Java(供 Kotlin 使用) 1、不得使用硬关键字 不要使用 Kotlin 的任何硬关键字作为方法的名称 或字段。允许使用 Kotlin 的软关键字、修饰符关键字和特殊标识…...
dify打造数据可视化图表
一、概述 在日常工作和学习中,我们经常需要和数据打交道。无论是分析报告、项目展示,还是简单的数据洞察,一个清晰直观的图表,往往能胜过千言万语。 一款能让数据可视化变得超级简单的 MCP Server,由蚂蚁集团 AntV 团队…...
回溯算法学习
一、电话号码的字母组合 import java.util.ArrayList; import java.util.List;import javax.management.loading.PrivateClassLoader;public class letterCombinations {private static final String[] KEYPAD {"", //0"", //1"abc", //2"…...
