打断behavior tree的异步动作节点，并执行其他节点动作

最新推荐文章于 2024-03-10 19:48:19 发布
最新推荐文章于 2024-03-10 19:48:19 发布
阅读量986 收藏 6
点赞数 4
分类专栏： ROS 2
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_45710350/article/details/109718775
版权

系列文章目录

思岚激光雷达rplidar从ROS 1到ROS 2的移植
ROS 2下navigation 2 stack的构建
订阅rviz2的导航目标位置消息“/goal_pose”
为Navigation 2创建自定义behavior tree plugin

文章目录

背景

在上一篇文章《为Navigation 2创建自定义behavior tree plugin》中，介绍了自定义behavior tree的action node “SpinGO”，使得机器人能够在循迹导航路径前，先原地旋转到和导航目标点基本一致的航向角上。该文章中的原地旋转只覆盖了机器人处于空闲状态下的原地旋转功能。如果机器人正处于一个活动中的导航过程，则原地旋转功能不能生效。

这是因为异步action node “FollowPath”决定的，先看behavior tree的结构：
在这里插入图片描述

黄色框中的异步action node “FollowPath”，其为“nav2_controller”服务的一个client，在behavior tree中被tick后，即是向“nav2_controller”服务发送了一个路径目标，controller服务将不断执行循迹的服务，直到完成。因此，当循迹服务运行时，“FollowPath”节点在behavior tree中返回的状态始终保持为“RUNNING”，当新的导航目标点给出，并基于该新目标生成新的路径后，“nav2_controller”将preempt原来的目标，然后继续执行新路径的循迹服务

上述原因可以查看action “FollowPath”及其基类“BtActionNode”确认。首先看action “FollowPath”、“BtActionNode”在整个导航中的位置和关系：
在这里插入图片描述

Behavior tree及其各个节点由类“BtNavigator”维护和控制，其中“bt_”即为behavior tree的句柄，由用户输入的“plug_lib_names”输入而构建，同时创建了在各个behavior tree nodes间共享并交换数据的“blackboard_”

“BtNavigator”一旦被导航目标激活，将通过“bt_”开始运行behavior tree及其各节点，代码片段如下：
在这里插入图片描述

当tick到“FollowPath”节点时，该节点将通过共享数据区“blackboard”查看是否有目标路径，以及路径是否被preempted，代码片段如下：
在这里插入图片描述

同时，如果出现preempted的情况，“FollowPath” action将向nav2_controller服务发送更新后的新路径，代码片段如下：
在这里插入图片描述
在这里插入图片描述

因此，“FollowPath”是一个异步运行的action节点，且只有当其完成导航任务后才会返回，无论是SUCCEEDED或者FAILED。之前加入behavior tree用于原地旋转的action node “SpinGO”也只有等“FollowPath”及其父节点返回，重新开始根节点的遍历后才会被tick，这就是前述原地旋转只有在机器人空闲时才有效的原因

1. 构建打断异步action node的behavior tree

能让在导航目标被preempted后，先原地旋转，然后再开始循迹的思想很朴素，在代码中的实现其实就是if then的结构，即下面伪代码逻辑：

IF there are new goal and new path
  THEN spin to goal orientation

follow path to goal

但是目前BehaivorTree.CPP似乎没有提供if then else这样的控制结构，幸运的是BehaviorTree.CPP有“ReactiveSequence”的控制结构，该结构能够打断异步执行的action node去查询条件状态，如下图示例：
在这里插入图片描述

该过程中“ApproachEnemy”是一个长时间运行的异步action node，其执行过程中始终返回RUNNING状态，而其依赖于前方是否有敌人，如果有则不断向敌人靠近，如果没有则终止该过程

NOTE：ReactiveSequence详细说明参考官网连接：
https://www.behaviortree.dev/sequencenode/

“ReactiveSequence”加上条件condition node实现了if判断的过程，但then和else的动作过程没有覆盖。在BehaviorTree.CPP中fallback控制结构能够实现对执行目标进行选择的逻辑，如下图示例：
在这里插入图片描述

该过程首先判断门是否已经打开，如果门已经打开，则返回SUCCEEDED，如果为关闭状态则尝试开门，如果开门的尝试失败，则尝试解门锁，如果解门锁的尝试失败，则尝试把门砸开，直到找到一个能够成功的节点为止

NOTE：Fallback详细说明参考官网连接：
https://www.behaviortree.dev/fallbacknode/

本文的目标：条件为是否有新的goal和path生成，如果有则进行向目标点的旋转，然后再开始导航循迹，即通过“Fallback”来选择是否执行“SpinGO”，然后再与“ReactiveSequence”组合形成if then的结构，如下图：
在这里插入图片描述

上图中“GoalPathUpdated”为自定义的condition node，用于判断是否有新的导航目标以及路径，“SpinGO”为自定义的action node，用于控制机器人原地旋转以面向导航目标点的航向角。其中，“GoalPathUpdated”前加入了“Inverter”节点，这是因为我们需要只有在新的导航点和路径存在后，才执行原地旋转动作，而“GoalPathUpdated”判断有新的导航点和目标后返回的是SUCCEEDED；此外，“Fallback”需要前面节点返回FAILED后，才执行下一个节点，所以取了一个反向关系。又因为“FollowPath”是一个长期运行的异步action node，因此需要不断的被执行，所以在“Fallback”前加入了“ForceSuccess”以保证“ReactiveSequence”的条件始终为SUCCEEDED，来执行“FollowPath”

2. 构建自定义condition node

区别于上一篇文章《为Navigation 2创建自定义behavior tree plugin》中构建behavior tree的action node的过程，condition node不需要service的支持，可以独立存在并被动态加载

本说明基于苇航智能的导航package，即“motion_ctrl_nav”，创建action nodes，也可以单独创建package。在“motion_ctrl_nav”的根目录下，创建文件夹“plugins”，用于存储behavior tree的plugins，如下图：
在这里插入图片描述

分别在目录“include/<package>/condition”和“plugins/condition”中创建文件“goal_path_updated_condition.hpp”和“goal_path_updated_condition.cpp”，添加如下代码：

HPP：

/******************************************************************
check if the goal and path are updated, an condition plugin for bt_navigator

Features:
- get current goal and path from blackboard
- check if the goal and path are updated
- xxx

Written by Xinjue Zou, xinjue.zou@outlook.com

GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.

Changelog:
2020-11-05: Initial version
2020-xx-xx: xxx
******************************************************************/
#pragma once
#include <string>
#include "behaviortree_cpp_v3/condition_node.h"


namespace nav2_behavior_tree
{

class GoalPathUpdatedCondition : public BT::ConditionNode
{
public:
  GoalPathUpdatedCondition(
    const std::string & condition_name,
    const BT::NodeConfiguration & conf);

  GoalPathUpdatedCondition() = delete;

  BT::NodeStatus tick() override;

  static BT::PortsList providedPorts()
  {
    return {};
  }
};

}  // namespace nav2_behavior_tree

CPP：

/******************************************************************
check if the goal and path are updated, an condition plugin for bt_navigator

Features:
- get current goal and path from blackboard
- check if the goal and path are updated
- xxx

Written by Xinjue Zou, xinjue.zou@outlook.com

GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.

******************************************************************/
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "motion_ctrl_nav/plugins/condition/goal_path_updated_condition.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace nav2_behavior_tree
{

GoalPathUpdatedCondition::GoalPathUpdatedCondition(
  const std::string & condition_name,
  const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf)
{
}

BT::NodeStatus GoalPathUpdatedCondition::tick()
{
  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");

  auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal");
  auto current_path = config().blackboard->get<nav_msgs::msg::Path>("path");
  static auto last_goal = current_goal;
  if (last_goal != current_goal && current_goal.pose == current_path.poses.back().pose)
  {
    last_goal = current_goal;
    
    //RCLCPP_INFO(node->get_logger(), "goal changedddddddddddddddddd %.3f %.3f", last_goal.pose.position.x, last_goal.pose.position.y);
    
    return BT::NodeStatus::SUCCESS;
  }
  else
  {    
    return BT::NodeStatus::FAILURE;
  }
}

}  // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<nav2_behavior_tree::GoalPathUpdatedCondition>("GoalPathUpdated");
}

仔细观察上述代码并对比《为Navigation 2创建自定义behavior tree plugin》中action node的代码可以发现，自定义condition node的继承关系为ConditionNode，因为condition不需要service，因此其被tick时的函数变为了“tick”，而action node由于需要service的缘故，其tick为继承并重载基类“BtActionNode”的“on_tick”函数

同样，在cpp文件末尾需要注册该condition node，提供node的名称，以能够被动态的加载，这里我们自定义为“GoalPathUpdated”，该名称将会是后续behavior tree描述文件中节点的关键字

3. 修改CMakeLists

为新的condition node加入编译目标，如下图：
在这里插入图片描述

该目标名称将会是后续导航参数文件需要为BtNavigator指定的动态加载的插件名称

NOTE：关于node安装在CMakeLists中的语句，请参考《为Navigation 2创建自定义behavior tree plugin》已经做过的说明

4. 修改behavior tree

将“FollowPath”部分加入上述behavior tree的层级结构，如下图：
在这里插入图片描述

TIP：更简单的方式是通过behavior tree的可视化编辑工具Groot，编辑然后导出xml文件。Groot的使用超出本内容范围，不在此累述，详细内容可以参看官网链接：
https://github.com/BehaviorTree/Groot

5. 修改导航参数文件yaml

导航过程由BtNavigator server完成，其依赖的behavior tree配置在导航参数yaml文件中指定。需要为其指定自定义condition node的编译目标名称“nav2_goal_path_updated_condition_bt_node”，该目标为CMakeLists中指定的编译目标，可参见上述章节内容，如下图：
在这里插入图片描述

6. 编译

完成上述步骤后，编译，在终端输入命令：

cd ~/dev_ws
colcon build --packages-select motion_ctrl_nav

7. 测试验证

现在可以验证自定义的behavior tree逻辑是否生效。首先，运行机器人，这里以苇航智能NaviBOT A0为例，在终端输入命令：

cd ~/dev_ws
sudo su
. install/setup.bash
ros2 launch motion_ctrl NaviBOT_A0.launch.py

其次，运行remote导航stack，在新终端输入命令：

cd ~/dev_ws
. install/setup.bash
ros2 launch motion_ctrl_nav navigation_remote_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yaml

在rviz中为机器人设置初始位置以激活导航，并设置导航目标点，查看运行导航的终端，将看到自定义action plugin “SpinGO”计算得到了需要旋转的角度，并开始执行旋转动作，如下图：
在这里插入图片描述

验证导航运行过程中，FollowPath被打断，SpinGO被执行的情况。在导航还未完成时，在rviz2中给出另一个导航命令，这时机器人将执行旋转的动作后，然后开始导航路径循迹，对应终端输出如下图：
在这里插入图片描述

总结

至此，基于本文及上一篇文章《为Navigation 2创建自定义behavior tree plugin》中说明方法，将能使大部分机器人的自定义动作逻辑都通过behavior tree的机制实现

behavior tree作为ROS 2 Navigation 2架构下导航动作的逻辑层次控制结构，其取代了ROS 1下的FSM，即状态机机制。behavior tree引入后，逻辑控制变得更加灵活，更便于代码的迭代更新，机器人的动作逻辑定义则完全可由开发者自行定义和实现

behavior tree机制提供了多种逻辑控制方式，本文中描述的方式为实现文中目标的其中之一，在behavior tree的机制下，还有很多组合能够达到和本文过程一致的目标，留给读者探索和发现

如果文中有错误和不明确的地方，欢迎指正，共同交流，xinjue.zou.whi@gmail.com

XinjueTM
关注
  • 4
    点赞
  • 踩
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
Unity3D 编辑器行为树节点样例_BehaviorTree
11-01
behaviourtree编辑器样例,可以自己修改扩展成自己项目的配置内容
BehaviorTree.js：行为树JavaScript实现
02-03
BehaviorTree.js 行为树JavaScript实现。 它们对于实现AI很有用。 如果您需要有关行为树的更多信息，请查看 ， 不错的。 产品特点 需要的：序列，选择器，任务 扩展：装饰器 安装 如果使用npm： npm install ...
BehaviorTree.CPP行为树学习：Reactive Trees
weixin_44979075的博客
09-21 373
BehaviorTree.CPP行为树学习：Reactive Trees
【BehaviorTree.CPP行为树学习：】并发和异步节点Concurrency and Asynchrous Nodes
weixin_44979075的博客
09-26 1069
【BehaviorTree.CPP行为树学习：】并发和异步节点Concurrency and Asynchrous Nodes
Behavior Tree行为树基础
一条咸鱼的博客
11-21 315
在Tasks下选择行为，添加到Behavior Tree中 selector：从左至右，其中一个子任务为true则返回true，停止遍历（相当于OR） sequence：从左至右，有一个子任务为false则返回false，停止遍历（相当于AND） return failure：无论其子任务是否成功执行，结果都返回false ...
BehaviorTree.CPP行为树学习：ReactiveFallback
weixin_44979075的博客
09-20 839
BehaviorTree.CPP行为树学习：ReactiveFallback
从零开始的node.js-1 基础格式、运行、打断、路由、读取文件。
qq_38674970的博客
10-17 121
//打算学下后端，可是不知道选php还是node，想了想反正不打算精通只是不想和后端交流过于麻烦而已，所以还是学node。js吧，不知道能不能学会 //1 安装node.js  不说啥了  2. 在js里写出如下代码  打开命令行 输入node xxx.js  即可看出打印的内容   再打开浏览器 输入打印出来的网址就可以看到第一条信息啦   3. 当你重新编写程序时，是不能直接运行的，可以...
BehaviorTree.CPP:C ++中的行为树库。 含电池
04-27
与其他实现相比，很少有功能可以使BehaviorTree.CPP变得唯一： 它使异步Action （即非阻塞）成为一等公民。 您可以构建可同时执行多个操作的React式行为。 树是使用域特定脚本脚本语言（基于XML）定义的，并且可以...
BehaviorTree
09-03
Unity 实现
行为树(Behavior trees)
念去去
07-08 5970
文章目录行为树组合节点次序节点选择节点并行节点修饰节点逆变节点成功节点重复节点叶子节点 行为树 行为树由多种不同类型的节点组成，这些节点都会返回三种状态中的一种作为节点的运行结果。三种状态分别是： 成功 - Success 失败 - Failure 运行中 - Running 前两个，正如它们的名字一样，是用来向它们的父节点通知运行的成功或失败。第三种是指还在运行中，结果还未确定，会在下一个 Tick 的时候再去检查这个节点的运行结果。这个功能非常重要，它可以让一个节点持续运行一段时间来维持某些行为。比
ros行为树-概念(基于behaviortree_v3)
Hiyu_的博客
11-29 1098
三个状态 SUCCESS,FAILURE,RUNNING，节点运行成功后返回给父节点状态。 RUNNING用于asynchronous(异步)节点里，当该节点需要更多时间执行时就返回该值。 同步（synchronous）与异步（asynchronous): 同步方法调用一旦开始，调用者必须等到方法调用返回后，才能继续后续的行为。 异步方法调用更像一个消息的传递，一旦开始，方法调用就会立即返回，调用者就可以继续后续的操作。而异步方法通常会在另外一个线程中“真实”地执行。整个过程，不会阻碍调用者的工作。对于调
行为树BehaviorTree学习记录4_异步动作
Leo@Apas
12-11 110
行为树的异步行为
行为树初识（Behavior Tree）
qq_34182164的博客
04-11 321
从核心来讲，BT是由一小部分简单的组件构成的。从形式上来讲，BT是一个有向的根树，其内部节点称之为控制流节点，叶节点称之为执行节点。对于每个连接的节点，通常使用术语父节点和子节点进行描述。根就是没有父节点的节点，所有其他的节点都有一个父节点。控制流节点至少有一个子节点。BT从生成给定频率的时钟信号的根节点开始执行。这些信号允许执行一个 节点，并传递给选中的一个或者多个子节点。当且仅当接受到信号才会执行该节点。
Groot行为树UI编辑器安装与问题解决（完美教程！）
Netceor的博客
03-12 2488
屏蔽Anaconda这一步很重要，血泪教训！！！不然后面会出现一堆错误，如果发现过程很不顺利，可以检查一下自己有没有彻底退出anaconda，重新下载编译一下首先是退出当前的环境，控制台前不要有括号暂时先屏蔽一下任何与anaconda相关的路径，不然后面可能会出现库的冲突更新完系统变量后一定要重启控制台，可以测试输入一下“python”，看现在是不是已经找不到python或者没有conda命令了，说明路径上成功屏蔽了。
ROS2 Navigation 进阶教程学习笔记 第二章
PC2721的博客
01-24 1516
ROS2 Navigation进阶教程第二章学习笔记
behavior tree 介绍
最新发布
sunsundayZ的博客
03-10 1308
从根节点开始按具体的顺序遍历执行，直到返回最终状态•叶节点（Execution Nodes）与底层代码结合起来，会执行具体的任务（action）或者检查（condition），返回 (success, failure, or running)•控制结点接受其子结点返回的状态，并决定下一个将展开哪个结点当只有一个目标点A但上述考虑是不完全的，我们在执行GoToA之前我们要先检测。例如当机器人已经到目的A时，就不需要再执行动作GoToA。直接FoundObj。
行为树（Behavior Tree）实践（2）– 进一步的讨论
12-06 5964
原文：http://www.aisharing.com/archives/99 上次提到了一些行为树的基本概念，包括行为节点，控制节点（选择，序列，并行），这次来更多，更深入的讨论行为树的一些东西，如果对行为树不是很了解，请参看这里。 一. 关于选择节点的讨论 我们说过选择节点的定义是通过判断子节点的前提条件来选择一个节点执行，这就牵涉到判断顺序的问题，是自左向右，还是随
BehaviorTree.CPP基础知识
weixin_42638851的博客
09-23 929
BehaviorTree.CPP行为树基础知识
JavaScript数据结构——树的学习总结
gggggayu的博客
09-25 291
JavaScript数据结构——树的学习，主要对树的常见操作进行了总结
介绍一下BehaviorTree CPP的所有节点
03-14
BehaviorTree CPP 是一个行为树库，提供了多种节点类型，包括： 1. SequenceNode：顺序节点，按照顺序执行子节点，直到有一个子节点返回失败或者所有子节点都返回成功。 2. SelectorNode：选择节点，按照顺序执行子节点，直到有一个子节点返回成功或者所有子节点都返回失败。 3. ParallelNode：并行节点，同时执行所有子节点，直到所有子节点都返回成功或者有一个子节点返回失败。 4. DecoratorNode：修饰节点，对子节点进行修饰，比如 InverterNode 反转子节点的返回值，RepeaterNode 重复执行子节点等。 5. ActionNode：行为节点，执行具体的行为，比如 MoveToNode 移动到指定位置，AttackNode 攻击指定目标等。 6. ConditionNode：条件节点，判断某个条件是否成立，比如 IsEnemyNearNode 判断是否有敌人在附近，IsHealthLowNode 判断自己的血量是否低于某个值等。 以上就是 BehaviorTree CPP 的所有节点类型。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值