ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理

news2025/1/11 18:52:15

在这里插入图片描述

   本文主要介绍ROS导航包Navigation中的 Movebase节点中的路径规划的相关流程，并对其进行梳理概括，同时本文也是《ROS局部路径规划器插件teb_local_planner规划流程概括总结》部分的前述文章。

在这里插入图片描述

   1、接收到目标点信息goal

   在接收到目标点goal之后，Move_base节点会调用回调函数goalCB，开始工作

goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

   2、goalCB函数

   goalCB函数仅仅将传入的 geometry_msgs:: PoseStamped 形式的goal转换成move_base_msgs:: MoveBaseActionGoal形式，再发布到对应类型的goal话题中，真正的执行函数是在构造函数里，actionlib server注册的executeCb函数。

 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)

   3、executeCb函数

   executeCb函数是move_base中路径规划的起始函数。将上述目标点传入到executeCb函数。

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)

   （1）在executeCb函数中，首先需要判断传入目标点的四元数的合法性，包括四元数是否完整，是否趋于0，以及进行规范化等，

   （2）然后将该坐标转换到世界坐标系下，同时通过publisher发布该坐标用于可视化显示。

   （3）随后唤醒规划线程，通过参数controller_frequency设置控制频率，开启代价地图costmap更新，并重置时间标志位

   注：在唤醒规划线程的程序中，执行了planThread函数的线程启动，该步骤会调用planThread函数。该过程不是通过函数调用的形式实现的，而是通过线程开关的形式实现的，在planThread函数中调用了全局规划器插件进行了全局规划，planThread函数将在本文第4部分进行介绍

lock(planner_mutex_)  

boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//给该线程上锁
planner_goal_ = goal;//传入目标并开启线程
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();//打开互斥锁

   （4）完成以上步骤后，程序进人while循环，按照上述设定的频率controller_ frequency进行循环，在循环中会进行如下操作：

   ①：根据标志位判断是否更改循环的控制频率

if(c_freq_change_)

r = ros::Rate(controller_frequency_)

   ②：根据标志位isPreemptRequested判断是否有新的目标点，若有新的目标点，则需要重新执行上述的（1）~（3）中描述的过程。若无新的目标点，则重置当前状态，允许新的目标点进行抢占。

if(as_->isPreemptRequested())

   ③：判断目标点坐标系是否与全局坐标系相同，若不相同则需要进行转换

if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID())

   ④：调用executeCycle函数进行局部路径规划

   注：在executeCycle函数中调用了局部规划器插件进行了局部规划，executeCycle函数将在本文第5部分进行介绍

bool done = executeCycle(goal, global_plan);

   ⑤：判断是否结束循环，若executeCycle的返回值为真，则结束循环

if(done)   return;

   ⑥：计算本次循环已经花费的时间，用设定的控制频率（循环频率）对应的时间与已花费的时间之差，作为本次循环的休眠时间。通过ROS的sleep机制进行休眠。

   此外，如果控制频率过高的话会输出一个警告信息，当时控制频率高于实际运算频率的时候终端会一直报警告：Control loop missed its desired rate of %.4fHz… the loop actually took %.4f seconds

r.sleep();
 //make sure to sleep for the remainder of our cycle time
if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());

   循环执行上面的①~⑥步

   （5）若上述循环是否因为各种原因导致节点被终止，则需要对当前的状态进行一定的处理

   4、planThread函数

   planThread函数在executeCb函数唤醒规划线程时被调用

   该线程创建以后会因为 runPlanner_ 值为false而挂起。当executeCb中唤醒线程前一刻时runPlanner_被设置为true。在这之后就正常情况下面没有被设置为fale过

   所以该线程被唤醒以后就不再进入等待的循环中，而是一直在大循环执行， 每个大循环都会调用一次 makeplan函数来调用全局路径规划器进行全局路径规划，并重新把新路径装入planner_plan_容器中(更新了路径) ，在executeCB中没有意外发生时只会唤醒一次该线程，以后不再唤醒。会一致执行大循环每个循环下发一次控制速度，但是每次循环中的路径可能会由于该线程一直在跑的缘故而发生改变。

planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

void MoveBase::planThread()

   （1）在planThread函数中，首先设置目标点，清空路径存储容器

geometry_msgs::PoseStamped temp_goal = planner_goal_;
...
planner_plan_->clear();

   （2）接着是本函数的核心部分，调用了makePlan函数，进而调用全局路径规划器插件进行全局规划，如果成功，结果将储存进planner_plan_中。所有的全局路径规划器插件必然包含一个makePlan函数提供给move_base，从而在此处进行调用。

bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

planner_->makePlan(start, goal, plan)

   （3）若得到了可用的全局路径，则将标志位new_global_plan_ 置为ture，供局部路径规划使用

new_global_plan_ = true;

   5、executeCycle函数

   executeCB是路径规划的启动函数，其过程中通过调用完成了全局路径的规划，executeCB函数在上文介绍的3 -（4）- ④ 步骤中调用的executeCycle是局部路径规划的正式执行函数，在executeCycle函数中的setplan函数中将调用局部路径规划器插件进行局部路径规划。

bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan)

   （1）在executeCycle函数中，首先获取机器人当前时刻在世界坐标系的位姿，并传递给feedback进行反馈和发布。

getRobotPose(global_pose, planner_costmap_ros_);

   （2）振荡检查，机器人每运动一段距离会记录一下当前时间以及位姿，其中current_ position是当前位姿，oscillation_pose_是上一个进入该判断时的位姿，参数oscillation_distance_是常数，默认值为0.5m

if(distance(current_position, oscillation_pose_) >= oscillation_distance_)

   （3）检查代价地图近期是否得到了更新。如果长时间没有更新，意味着机器人失去了周围的感知能力， 此时移动机器人是危险的，通过publishZeroVelocity 控制机器人停下来并退出运动逻辑。

if(!controller_costmap_ros_->isCurrent())

   （4）判断是否得到的新的全局路径，即new_global_plan_ 的值是否为True，若是，则调用setplan函数，储存全局路径，进而在需要的时候传递给局部路径规划使用，所有的局部路径规划器都应该有一个setplan函数作为接口供move_base进行调用来传递全局路径信息。

if(new_global_plan_)

if(!tc_->setPlan(*controller_plan_))

   （5）在将路径传入给局部路径规划函数后，executeCycle函数根据当前状态state_进行下一步的控制。

switch(state_)

   state_的状态分为三种：PLANNING、CONTROLLING、CLEARING。state_在全局路径规划之前被设置为PLANNING，代表当前处于路径规划阶段；然后在全局路径规划函数planThread中，完成全局路径规划并得到路径后被设置为CONTROLLING，代表当前路径成功，进入控制模式使机器人到达目标点；而在全局路径规划失败或者局部路径规划失败且超过重试次数时都会被置为CLEARING，代表取消当前规划。

   ①、若当前状态为PLANNING，说明发生的异常情况。全局路径规划没有完成就开始执行局部路径规划了，这时候算法会重新启动全局路径规划线程。
   注：（可在导航包中查询lock(planner_mutex_) 来查询启动全局规划的地方）

   ②、若当前状态为CLEARING，则全局路径规划失败了或者局部路径规划失败了。此时要考虑是否进行重新规划。

   如果重试的次数recovery_index_小于设置的次数recovery_behaviors_.size()，则调用全局路径规划看有没有新的路可以走。否则，根据设置的参数变量recovery_trigger_的状态进行对应的报错。

if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())

   ③、若当前状态为CONTROLLING，也就是我们希望的正常状态，则先判断机器人是否到达目标点

if(tc_->isGoalReached())

   在LatchedStopRotateController::isGoalReached函数中，首先判断了当前坐标与目标点之间的距离是否小于设定值xy_goal_tolerance。如果满足该要求说明机器人已经在目标点附近，这时候算法再判断机器人的角度值是否复合设定阈值yaw_goal_tolerance，如果两个条件都满足说明机器人已经到达目标点。则返回true。则move_base的运动会设置为Succeeded。

   判断机器人是否处于震荡中，

 if(oscillation_timeout_ > 0.0 && last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())

   若是，则发布将速度置为0的指令，并将状态置为CLEARING

   计算机器人的速度，通过调用局部规划器的computeVelocityCommands函数实现

if(tc_->computeVelocityCommands(cmd_vel))

   这里的computeVelocityCommands函数是move_base调用局部路径规划器插件进行局部规划的接口函数，每个局部路径规划器插件都要有一个computeVelocityCommands函数作为被调用的接口

   如果规划成功，则下发速度到cmd_vel；

   如果规划失败，此时如果如果距离上次成功的规划超过了容忍时间controller_patience_则将机器人停止，取消规划发布错误信息，如果此时还没超出容忍时间，则算法会再接受一下当前的状态，将速度改为0同时设置state_为PLANNING重新进行一次全局路径规划

在这里插入图片描述

   总的来说或Move_base在接收到目标点信息后，会通过回调函数，调用goalCB函数进行目标点格式的转换，真正的执行函数是构造函数里actionlib server注册的executeCb函数。executeCb函数也是move_base节点的主循环函数，按照设定的频率进行规划，executeCb函数启动规划线程的时候会调用planThread函数，而planThread函数中会调用 makeplan函数进行全局路径规划，也就是move_base节点调用全局规划器插件的接口函数 。

   executeCb函数还会调用executeCycle函数函数，通过setplan函数将得到的全局路径传递给局部路径规划器，在这个函数中会根据机器人当前的状态进行进一步处理，若为CONTROLLING，则说明为正常状态，调用 computeVelocityCommands函数进行局部路径规划 ，这个函数也是move_base节点调用局部规划器插件的接口函数。

   对于异常的PLANNING和CLEARING状态，则会根据情况决定是否要调用planThread函数重新尝试进行全局规划。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/482405.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

JAVA医院管理云HIS统计报表子系统、系统管理字系统功能实现

JAVA医院管理云HIS统计报表子系统、系统管理字系统功能实现

一、统计报表子系统 统计报表子系统功能模块&#xff1a;包括门诊收入汇总、住院收入汇总、收费统计报表、收费明细报表、 缴款日报、门诊收费汇总、住院科室日志、住院结算汇总、医疗项目统计、检查项目统计、 检验项目统计、月末收支汇总、药品进销存统计。 &#xff08;1…
阅读更多...
从零开始三端口DC-DC变换器

从零开始三端口DC-DC变换器

1、 题目解析 基本要求 &#xff08;1&#xff09; U S 50 V 、 I O 1.2 A U_S50V、I_O1.2A US​50V、IO​1.2A 条件下&#xff0c;变换器工作在模式I&#xff0c; U O 30 V 0.1 V &#xff0c; I B ≥ 0.1 A U_O30V0.1V&#xff0c;I_B≥0.1A UO​30V0.1V&#xff0c;IB​…
阅读更多...
CleanMyMac X4.13.2最新版下载

CleanMyMac X4.13.2最新版下载

现在cleanmymac x4.13.2中文版是大家首选的优秀mac清理软件。CleanMyMac集合了多种功能&#xff0c;几乎可以满足用户所有的清洁需求。它不仅包含各种清理功能&#xff0c;还具有卸载、维护、扩展、碎纸机等实用功能&#xff0c;可同时替代多种工具。它可以清理、优化、维护和监…
阅读更多...
边缘人工智能——nanodet模型实践指引，从标注数据集到实现部署文件

边缘人工智能——nanodet模型实践指引，从标注数据集到实现部署文件

内容概述 首先获得一个合适的nanodet模型版本&#xff0c;配置nanodet适用的环境&#xff0c;然后对网上公开的生数据集进行重新标注&#xff0c;配置nanodet并进行训练&#xff0c;.pth到.onnx的模型转化及简化&#xff0c;编写推理文件。 文章着重于实践方向指引&#xff0c;…
阅读更多...
【LeetCode股票买卖系列:123. 买卖股票的最佳时机 III 暴力递归=＞记忆化搜索=＞动态规划】

【LeetCode股票买卖系列:123. 买卖股票的最佳时机 III 暴力递归=＞记忆化搜索=＞动态规划】

&#x1f680; 算法题 &#x1f680; &#x1f332; 算法刷题专栏 | 面试必备算法 | 面试高频算法 &#x1f340; &#x1f332; 越难的东西,越要努力坚持&#xff0c;因为它具有很高的价值&#xff0c;算法就是这样✨ &#x1f332; 作者简介&#xff1a;硕风和炜&#xff0c;…
阅读更多...
基于Flask+Bootstrap+机器学习的南昌市租房价格预测系统

基于Flask+Bootstrap+机器学习的南昌市租房价格预测系统

&#x1f935;‍♂️ 个人主页&#xff1a;艾派森的个人主页 ✍&#x1f3fb;作者简介&#xff1a;Python学习者 &#x1f40b; 希望大家多多支持&#xff0c;我们一起进步&#xff01;&#x1f604; 如果文章对你有帮助的话&#xff0c; 欢迎评论 &#x1f4ac;点赞&#x1f4…
阅读更多...
图片分类：精细化分类，(Fine-Grained Categorization) 基于人的行为的精细化分类

图片分类：精细化分类，(Fine-Grained Categorization) 基于人的行为的精细化分类

文字大纲 简介数据集常用数据集方法1 ： 强监督方法2 ： 弱监督Two Level Attention Model双线性网络 Bilinear CNN model参考文献和学习路径简介 细粒度图像识别 (fine-grained image recognition)，即 精细化分类。 细粒度图像分类(Fine-Grained Categorization), 又被称作…
阅读更多...
2023年05月IDE流行度最新排名

2023年05月IDE流行度最新排名

点击查看最新IDE流行度最新排名&#xff08;每月更新&#xff09; 2023年05月IDE流行度最新排名 顶级IDE排名是通过分析在谷歌上搜索IDE下载页面的频率而创建的 一个IDE被搜索的次数越多&#xff0c;这个IDE就被认为越受欢迎。原始数据来自谷歌Trends 如果您相信集体智慧&am…
阅读更多...
感知机介绍

感知机介绍

1&#xff0c;数学定义&#xff1a; Note:<>在数学中通常指求期望的意思。 假设我们用感知机区分cat和dog&#xff0c;使用下面三个特征&#xff1a;x1: color of hair&#xff1b;x2:length of leg&#xff1b;x3:volume of head。cat 用1表示&#xff0c;dog用-1表示&…
阅读更多...
Golang每日一练(leetDay0053)

Golang每日一练(leetDay0053)

目录 155. 最小栈 Min Stack &#x1f31f;&#x1f31f; 156. 二叉树的上下翻转 Binary Tree Upside Down &#x1f31f;&#x1f31f; &#x1f31f; 每日一练刷题专栏 &#x1f31f; Golang每日一练 专栏 Python每日一练 专栏 C/C每日一练 专栏 Java每日一练 专栏 …
阅读更多...
ArrayList集合扩容机制入门

ArrayList集合扩容机制入门

首先&#xff0c;ArrayList集合存储的数据在底层是一个数组&#xff08;名字elementData&#xff09;&#xff0c;这个数组是Object的数组&#xff0c;因为是Object数组&#xff0c;所以集合啥都可以装。 讲解ArrayList的扩容机制&#xff0c;要从ArrayList的构造器来分类&…
阅读更多...
Spark RDD 持久化（CheckPoint 检查点）

Spark RDD 持久化（CheckPoint 检查点）

RDD Cache 缓存 RDD 通过 Cache 或者 Persist 方法将前面的计算结果缓存&#xff0c;默认情况下会把数据以缓存 在 JVM 的堆内存中。但是并不是这两个方法被调用时立即缓存&#xff0c;而是触发后面的 action 算 子时&#xff0c;该 RDD 将会被缓存在计算节点的内存中 // cach…
阅读更多...
debian11快速 ceph集群17.2.6（Quincy版）

debian11快速 ceph集群17.2.6（Quincy版）

由于网友跟我讲Pacific版快到期了&#xff0c;所以出一个Quincy版的部署文档 配置一下源 echo "deb http://mirrors.163.com/ceph/debian-quincy/ bullseye main" > /etc/apt/sources.list.d/ceph.list还是像以前一样使用docker或者podman 安装工具cephadm ce…
阅读更多...
java获取文件名后缀方法

java获取文件名后缀方法

Java是一种应用广泛的编程语言&#xff0c;可以通过多种方式来实现对文件的操作。如文件名后缀、文件扩展名等。今天我们来看下 Java是如何获取文件名后缀的吧&#xff01; 1.打开一个空文件&#xff0c;将其复制到一个新的文件夹中。 2.新建一个类&#xff0c;在里面定义方法&…
阅读更多...
mysql 数据库备份

mysql 数据库备份

目录 数据库备份的方式 一、备份整个 $datadir 二、用mysqldump备份 备份某个库 只备份某个库下某个表 有很多库时候&#xff0c;一次性备份所有的库 一次指定备份某几个库 只备份表结构&#xff0c;不要里面数据 数据库还原的方式 1、在对应数据库下source还原 2…
阅读更多...
学系统集成项目管理工程师（中项）系列16a_风险管理（上）

学系统集成项目管理工程师（中项）系列16a_风险管理（上）

1. 风险的定义 1.1. 损失的不确定性 1.1.1. 狭义 1.2. 带来损失的可能性&#xff0c;也指可能获利的机会 1.2.1. 广义 1.3. 风险是一种不确定的事件或条件&#xff0c;一旦发生&#xff0c;就会产生积极或消极的影响 2. 性质划分 2.1. 纯粹风险 2.1.1. 只有损失可能性而…
阅读更多...
IntelliJ IDEA修改背景颜色大全（护眼绿等）设置注释颜色

IntelliJ IDEA修改背景颜色大全（护眼绿等）设置注释颜色

一.IDEA默认有3种背景颜色 路径为File->settings->Editor->Color Scheme可以设置软件默认颜色&#xff0c;旁边的小齿轮添加颜色名字 二.IDEA扩展颜色&#xff08;护眼绿&#xff09; 第一种方法&#xff1a; IDEA设置一张背景图片,路径&#xff1a;File->Setti…
阅读更多...
C#，生信软件实践（02）——欧洲分子生物学实验室（EMBL格式文件）转为核酸序列或多肽序列（FASTA格式文件）的源代码

C#，生信软件实践（02）——欧洲分子生物学实验室（EMBL格式文件）转为核酸序列或多肽序列（FASTA格式文件）的源代码

>生信老白写的基础代码.fasta MAYBENOANYUSAGE 1 EMBL 1.1 EMBL组织 欧洲分子生物学实验室EMBL&#xff08;European Molecular Biology Laboratory&#xff09;1974年由欧洲14个国家加上亚洲的以色列共同发起建立&#xff0c;现在由欧洲30个成员国政府支持组成&#xf…
阅读更多...
【ARMv8 编程】A64 内存访问指令——内存存储指令

【ARMv8 编程】A64 内存访问指令——内存存储指令

在内存加载一节中实际上已经使用了内存存储指令了&#xff0c;内存存储指令将寄存器的值存储到内存中。 同样&#xff0c;Store 指令的一般形式如下&#xff1a; STR Rn, <addr> 还有 unscaled-offset 偏移形式&#xff0c;例如 STUR<type>。 程序员通常不需要明…
阅读更多...
Python多元线性回归预测模型实验完整版

Python多元线性回归预测模型实验完整版

多元线性回归预测模型 实验目的 通过多元线性回归预测模型&#xff0c;掌握预测模型的建立和应用方法&#xff0c;了解线性回归模型的基本原理 实验内容 多元线性回归预测模型 实验步骤和过程 (1)第一步&#xff1a;学习多元线性回归预测模型相关知识。 一元线性回归模型…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023