tick是从树的根节点(或起点)到其子节点的信号,敦促它们执行任务。节点仅在收到即时报价时“采取行动”。
节点开始执行其任务后,它会使用以下三种状态之一与控制器(其父节点)通信其进度:正在运行、成功或失败。 根据这些状态,导演决定将下一个即时报价发送到何处。这是一个持续到任务完成或不再有效的循环。
行为树由两种主要类型的节点组成:控制流节点和执行节点。让我们更深入地了解它们。
控制流节点主要有四种类型:
序列(→)节点:这就像一个严格的助手,他要求每个任务都必须完美地完成,顺序,然后才能继续下一个任务。如果任何任务失败,序列将停止并报告失败。
回退 (?节点(也称为选择器):这是一个更灵活的助手,他有一个任务列表并逐个尝试,直到一个成功。如果所有任务都失败,则回退报告失败。
并行 (⇒)节点: 这是一个可以同时处理多项任务的助手,指导多个演员同时执行他们的任务。并行节点根据一组必须成功的任务来定义其成功。
装饰器节点:这个助手有点特别,因为它只处理一个演员,但可以通过各种方式修改演员的行为。有不同种类的装饰器,每种装饰器都为任务提供了独特的转折。
执行节点是参与者,即执行任务(操作)或评估某些条件(条件)的参与者。
操作节点: 当此参与者收到勾号时,它会启动其任务,使控制器保持“正在运行”状态的更新,最后报告“成功”或“失败”。
条件节点: 这个演员更像是一个抽查者。它评估是否满足某些条件,如果条件为真,则立即报告“成功”,如果条件为假,则立即报告“失败”。
请记住,在BT中,节点仅在收到信号或“滴答”时才起作用。一旦任务启动,机器人就会将其状态传达回其父节点。循环一直持续到所有任务完成或无法完成。
运行错误1:cmake出错:
解决办法:更换CmakeLists.txt
cmake_minimum_required(VERSION 3.5)
project(bt_example)
set(CMAKE_CXX_STANDARD 17)
find_package(behaviortree_cpp REQUIRED)
message(STATUS "Found behaviortree_cpp version: ${behaviortree_cpp_VERSION}")
add_executable(bt_example main.cpp)
target_link_libraries(bt_example behaviortree_cpp)
set_target_properties(bt_example PROPERTIES INSTALL_RPATH "$ORIGIN")
运行错误2:运行./bt_example报错:
解决办法:
看起来 libbehaviortree_cpp.so
库文件位于 BehaviorTree.CPP/build
目录下。为了让系统找到该库文件,你可以尝试以下几种方法:
-
使用 LD_LIBRARY_PATH: 设置环境变量
LD_LIBRARY_PATH
,让系统能够找到该库文件。假设libbehaviortree_cpp.so
的路径是/home/zzq/BTree/BehaviorTree.CPP/build/libbehaviortree_cpp.so
,可以运行以下命令:export LD_LIBRARY_PATH=/home/zzq/BTree/BehaviorTree.CPP/build/:$LD_LIBRARY_PATH
这会将
/home/zzq/BTree/BehaviorTree.CPP/build/
添加到系统共享库的搜索路径中。 -
复制库文件到系统库目录: 你也可以将
libbehaviortree_cpp.so
文件复制到系统默认的共享库目录中,比如/usr/lib
或者/usr/local/lib
:sudo cp /home/zzq/BTree/BehaviorTree.CPP/build/libbehaviortree_cpp.so /usr/lib/
这样操作可以让系统默认地找到该库文件。
尝试其中一种方法,看能否解决找不到 libbehaviortree_cpp.so
的问题。如果还有其他问题,可以继续尝试或提供更多细节。
example1:
运行结果:
example2:
#include <iostream>
#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/behavior_tree.h>
using namespace BT;
// 定义 FindBall 动作节点
class FindBall : public BT::SyncActionNode
{
public:
FindBall(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// 定义 FindBall 节点的 tick() 函数
NodeStatus tick() override
{
std::cout << "[⚓ FindBall ] => \t" << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
// 定义 PickBall 动作节点
class PickBall : public BT::SyncActionNode
{
public:
PickBall(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// 定义 PickBall 节点的 tick() 函数
NodeStatus tick() override
{
std::cout << "[⚓ PickBall ] => \t" << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
// 定义 PlaceBall 动作节点
class PlaceBall : public BT::SyncActionNode
{
public:
PlaceBall(const std::string& name) : BT::SyncActionNode(name, {})
{
}
// 定义 PlaceBall 节点的 tick() 函数
NodeStatus tick() override
{
std::cout << "[⚓ PlaceBall ] => \t" << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
// 定义 GripperInterface 类用于与机械手交互
class GripperInterface
{
private:
bool _opened;
public:
GripperInterface() : _opened(true)
{
}
NodeStatus open()
{
_opened = true;
std::cout << "GripperInterface::open" << std::endl;
return BT::NodeStatus::SUCCESS;
}
NodeStatus close()
{
std::cout << "GripperInterface::close" << std::endl;
_opened = false;
return BT::NodeStatus::SUCCESS;
}
};
// 定义 BallClose 条件函数
BT::NodeStatus BallClose()
{
std::cout << "[ Close to ball: NO ]" << std::endl;
return BT::NodeStatus::FAILURE;
}
// 定义 BallGrasped 条件函数
BT::NodeStatus BallGrasped()
{
std::cout << "[ Grasped: NO ]" << std::endl;
return BT::NodeStatus::FAILURE;
}
// 以 XML 格式定义行为树结构
static const char* xml_text = R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<FindBall name="found_ok"/>
<Sequence>
<Fallback>
<BallClose name="no_ball"/>
<PickBall name="approach_ball"/>
</Fallback>
<Fallback>
<BallGrasped name="no_grasp"/>
<GraspBall name="grasp_ball"/>
</Fallback>
</Sequence>
<PlaceBall name="ball_placed"/>
</Sequence>
</BehaviorTree>
</root>
)";
int main()
{
BehaviorTreeFactory factory;
// 使用工厂注册自定义动作节点
factory.registerNodeType<FindBall>("FindBall");
factory.registerNodeType<PickBall>("PickBall");
factory.registerNodeType<PlaceBall>("PlaceBall");
// 使用工厂注册自定义条件节点
factory.registerSimpleCondition("BallClose", std::bind(BallClose));
factory.registerSimpleCondition("BallGrasped", std::bind(BallGrasped));
// 创建 GripperInterface 实例
GripperInterface gripper;
// 使用 GripperInterface 实例注册简单动作节点
factory.registerSimpleAction("GraspBall", std::bind(&GripperInterface::close, &gripper));
// 使用 XML 描述创建行为树
auto tree = factory.createTreeFromText(xml_text);
// 运行行为树直到完成
tree.tickWhileRunning();
return 0;
}
您已经创建了三个操作节点:FindBall、PickBall和PlaceBall。这些类中的每一个都扩展SyncActionNode并实现函数tick(),该函数记录操作的名称并返回NodeStatus::SUCCESS。
您已经定义了一个类GripperInterface,该类表示机器人的抓手。它可以打开和关闭夹持器,两个操作返回NodeStatus::SUCCESS。
您已经定义了两个条件函数:BallClose和BallGrasped。这些功能表示机器人在捡球之前需要进行的检查。他们目前总是返回NodeStatus::FAILURE,模拟球没有接近并且机器人还没有抓住球的场景。
XML字符串定义行为树xml_text。它从一个序列节点开始,该节点首先尝试找到球(FindBall)。找到球后,它会尝试拾取球,这是由两个回退节点组成的另一个序列节点。第一个回退节点检查球是否接近 (BallClose),如果没有,机器人将接近球 (PickBall)。第二个回退节点检查球是否被抓住(BallGrasped),如果没有,机器人会抓住球(GraspBall)。一旦球被捡起,机器人就会放置球(PlaceBall)。
最后,main该函数注册自定义操作和条件节点,GripperInterface创建GraspBall的实例,使用GripperInterface注册操作,从 XML 文本创建树,并在树运行时勾选树。
运行结果:
运行错误3:
只能说很烦
#include "std_msgs/msg/string.hpp" // 导入用于发布字符串消息的头文件
写成了#include "std_msgs/msg/String.hpp" // 导入用于发布字符串消息的头文件
成功发布了/waypoint_topic,并且成功订阅上了。
navigation2-main\nav2_bt_navigator:
main.cpp:
main
函数是程序的入口点。rclcpp::init(argc, argv);
用于初始化ROS 2节点,解析命令行参数并配置ROS 2。std::make_shared<nav2_bt_navigator::BtNavigator>();
创建一个指向BtNavigator
类的shared_ptr
,这是一个ROS 2节点,代表了某种行为树导航器(Behavior Tree Navigator)。rclcpp::spin(node->get_node_base_interface());
在节点上启动事件循环,使节点保持运行状态。get_node_base_interface()
用于获取节点的基本接口。rclcpp::shutdown();
在程序结束时关闭ROS 2节点和其它资源。
bt_navigator.cpp:
BtNavigator
类是一个使用行为树来导航机器人到目标位置的操作服务器。
explicit BtNavigator(rclcpp::NodeOptions options = rclcpp::NodeOptions());
- 这是构造函数,用于创建
BtNavigator
类的实例。它接受一个rclcpp::NodeOptions
参数,允许节点创建时的自定义选项。
~BtNavigator();
- 析构函数,在对象生命周期结束时调用。这里执行对象的清理工作。
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override;
- 当节点被配置时调用。这个函数配置导航器插件的操作服务器,订阅了名为 "goal_sub" 的话题,并从 XML 文件构建行为树。
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override;
- 激活操作服务器的函数。在此函数中进行节点的激活工作。
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override;
- 停用操作服务器的函数。在此函数中进行节点的停用工作。
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override;
- 重置成员变量的函数。在此函数中进行节点的清理工作。
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override;
- 关机时调用的函数。在此函数中进行节点的关闭工作。
类中还包含了一些成员变量,如 class_loader_
用于加载导航器插件,navigators_
保存加载的导航器插件实例,plugin_muxer_
用于管理这些插件的执行。还有其他变量用于获取反馈的指标和处理节点使用的旋转变换。
BtNavigator::BtNavigator(rclcpp::NodeOptions options)
初始化节点:使用
nav2_util::LifecycleNode
的构造函数初始化节点,给定了节点的名称("bt_navigator")和空命名空间。同时,使用传入的options
对象中的automatically_declare_parameters_from_overrides(true)
设置一些节点选项,可能是自动从参数覆盖中声明参数的设置。初始化
class_loader_
对象:使用class_loader_
对象来加载NavigatorBase
类的插件。这个对象提供了节点加载插件的功能,这些插件包含了节点运行所需的行为树相关的功能和行为。记录节点创建信息:使用
RCLCPP_INFO(get_logger(), "Creating");
在日志中输出一条信息,标记节点正在被创建。声明和设置参数:使用
declare_parameter_if_not_declared()
方法确保在节点启动时,如果这些参数还没有被声明过,就会将它们声明并设置默认值。这些参数包括:
"plugin_lib_names"
:插件库的名称列表。"transform_tolerance"
:变换容差的值。"global_frame"
:全局坐标系的名称。"robot_base_frame"
:机器人基准坐标系的名称。"odom_topic"
:里程计数据的话题名称。
on_configure
这段代码实现了
BtNavigator
类中的on_configure
方法,在节点配置时被调用,执行以下功能:
记录节点配置信息:使用
RCLCPP_INFO(get_logger(), "Configuring");
在日志中输出一条信息,表示节点正在进行配置。创建 TF2 相关对象:初始化了
tf_
(一个tf2_ros::Buffer
对象)和tf_listener_
(一个tf2_ros::TransformListener
对象),用于监听并管理 TF 变换。这些对象用于处理机器人在不同坐标系下的位置和姿态信息。获取全局参数:从节点的参数服务器中获取全局参数,如全局坐标系名称、机器人基准坐标系名称、TF 容忍度和里程计主题。
设置反馈工具类:创建了
nav2_core::FeedbackUtils
对象feedback_utils
,用于提供 TF 相关信息给行为树节点的反馈使用。创建里程计平滑器对象:使用
nav2_util::OdomSmoother
类创建了odom_smoother_
对象,用于获取当前速度信息。Navigator默认设置:设定了一些默认的导航器标识和类型,若参数未被设置,则使用这些默认值。
加载导航器插件:根据设定的导航器标识,逐一加载导航器插件,并调用其配置方法,传递必要的参数和工具类实例。加载导航器插件是为了启用行为树中的导航行为,每个导航器可能提供不同的导航方式或功能。
总的来说,这个
on_configure
方法主要负责节点的初始化和配置,包括设置 TF2 相关对象、获取参数、创建工具类实例、加载导航器插件等,以便节点在运行过程中能够准备好所需的资源和配置。
流程:
Main 函数:启动创建一个
BtNavigator
类的实例,并初始化 ROS 2 节点。这个节点是整个导航系统的起点。BtNavigator 类:这个类负责整个导航器的配置和管理。在其中,它进行了节点的配置,加载了导航器插件,并为后续的导航提供了所需的环境和配置。
导航器插件:这些插件承担了具体的导航任务。它们会加载行为树。插件会从 XML 文件构建行为树的逻辑,执行导航任务。
XML 文件:导航器插件可以根据需要使用 XML 文件来实现导航行为。
导航操作:在导航器插件中,确实会执行一系列的导航操作,这可能涉及行为树的执行、传感器数据的处理、路径规划、执行控制指令等一系列任务,以实现机器人的导航目标。文章来源:https://www.toymoban.com/news/detail-783873.html
总体来说,导航系统通常是一个复杂的系统,涉及多个模块和组件的协同工作,以实现机器人的自主导航。文章来源地址https://www.toymoban.com/news/detail-783873.html
到了这里,关于骨架机器人P4——行为树demo的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!