打断behavior tree的异步动作节点，并执行其他节点动作
系列文章目录
思岚激光雷达rplidar从ROS 1到ROS 2的移植
ROS 2下navigation 2 stack的构建
订阅rviz2的导航目标位置消息“/goal_pose”
为Navigation 2创建自定义behavior tree plugin
文章目录
- 系列文章目录
- 背景
- 1. 构建打断异步action node的behavior tree
- 2. 构建自定义condition node
- 3. 修改CMakeLists
- 4. 修改behavior tree
- 5. 修改导航参数文件yaml
- 6. 编译
- 7. 测试验证
- 总结
背景
在上一篇文章《为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/
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, [email protected]
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
#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, [email protected]
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
******************************************************************/
#include
#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("node");
auto current_goal = config().blackboard->get("goal");
auto current_path = config().blackboard->get("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("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的机制下，还有很多组合能够达到和本文过程一致的目标，留给读者探索和发现
如果文中有错误和不明确的地方，欢迎指正，共同交流，[email protected]