骨架机器人P4——行为树demo

最新推荐文章于 2024-01-23 17:13:18 发布

zzqcays

最新推荐文章于 2024-01-23 17:13:18 发布

阅读量1.1k

点赞数 20

文章标签：机器人 linux 运维

本文链接：https://blog.csdn.net/weixin_53805189/article/details/135003323

版权

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 文件来实现导航行为。

导航操作：在导航器插件中，确实会执行一系列的导航操作，这可能涉及行为树的执行、传感器数据的处理、路径规划、执行控制指令等一系列任务，以实现机器人的导航目标。

总体来说，导航系统通常是一个复杂的系统，涉及多个模块和组件的协同工作，以实现机器人的自主导航。

zzqcays

关注

20
点赞
踩
15

收藏

觉得还不错? 一键收藏
1
评论
骨架机器人P4——行为树demo

tick是从树的根节点（或起点）到其子节点的信号，敦促它们执行任务。节点仅在收到即时报价时“采取行动”。或。根据这些状态，导演决定将下一个即时报价发送到何处。这是一个持续到任务完成或不再有效的循环。行为树由两种主要类型的节点组成：控制流节点和节点。让我们更深入地了解它们。：这就像一个严格的助手，他要求每个任务都必须完美地完成，顺序，然后才能继续下一个任务。如果任何任务失败，序列将停止并报告失败。这是一个更灵活的助手，他有一个任务列表并逐个尝试，直到一个成功。
复制链接

扫一扫