骨架机器人P5——预添加跟踪模块

最新推荐文章于 2024-11-12 16:01:45 发布

zzqcays

最新推荐文章于 2024-11-12 16:01:45 发布

阅读量1.3k

点赞数 16

文章标签：机器人

本文链接：https://blog.youkuaiyun.com/weixin_53805189/article/details/135438842

版权

文章讲述了如何在已完成骨架机器人与nav2导航结合的基础上，扩展追踪功能，通过修改nav2的行为树配置，集成跟踪模块，以实现对人员的实时跟踪。重点介绍了行为树的参数设置和导航器的配置，以及可能的追踪行为树xml结构的引入。

摘要由优快云通过智能技术生成

2024/1/7

在之前的基础上，已经完成了骨架机器人和nav2导航的结合，实现了nav2在骨架机器人上运行，所以接下来是为了实现在骨架机器人上运行跟踪模块，实现对人员的跟踪。

因为nav2的基础导航系统建立在bringup包上，根据navigation2-main\nav2_bringup\params的nav2_params.yaml，可获取一下的行为树配置代码：

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
        ...
      - nav2_is_battery_charging_condition_bt_node
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

其中：

1. 总体配置:

bt_navigator: 定义了行为树导航器的配置部分。
ros__parameters: 指明了以下参数是 ROS 参数。

2. 基础参数:

global_frame: 定义了全局坐标系的名称（通常为 "map"）。
robot_base_frame: 定义了机器人本体坐标系的名称（通常为 "base_link"）。
odom_topic: 指定了里程计数据的 ROS 话题名称。
bt_loop_duration: 设置了行为树的循环周期。
default_server_timeout: 设置了默认服务器超时时间。
wait_for_service_timeout: 设置了等待服务启动时的超时时间。
action_server_result_timeout: 设置了等待行为树节点执行结果的超时时间。

3. 导航器类型:

navigators: 列出了可用的导航器类型，通常包括：
navigate_to_pose：用于导航到单个目标点。
navigate_through_poses：用于导航一系列目标点。

4. 导航器插件:

navigate_to_pose 和 navigate_through_poses: 分别指定了对应导航器的插件名称。

5. 行为树节点库:

plugin_lib_names: 列出了行为树导航器所需的节点库名称，用于实现各种导航行为和条件判断。

6. 错误代码:

error_code_names: 列出了需要监控的错误代码名称，用于在导航过程中进行错误处理。

而这两个默认的导航器对应的xml文件如下：

navigate_to_pose：

navigate_through_poses：

navigate_to_pose对应的行为树：

所以为了实现跟踪，需要在此行为树的基础上加上跟踪的子树。

跟踪的子树

参考：nav2跟踪动态点

任务要求如下：

改变仅限于用于导航的行为树。该行为树可以在需要时在 NavigateToPose 行为中选择，也可以是默认行为树。它由可在运行时配置的插件组成。
规划器和控制器的配置不会改变。
该行为将无限期运行，直到被启动者取消。
对要跟随的动态物体（如人）的检测不在本教程的范围内。如图所示，您的应用程序应该为感兴趣的物体提供一个检测器，将初始位姿发送给 NavigateToPose 行为，并在任务期间更新它。您可以利用许多不同类型的检测器来实现这个应用程序：

跟踪动态点的行为树xml：

<!--
  This Behavior Tree follows a dynamic pose to a certain distance
-->

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <Sequence>
          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
            <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
          </GoalUpdater>
         <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
        </Sequence>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{truncated_path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>

此行为树以 1 Hz 重新规划一条新路径，并将该路径传递给控制器以遵循。

首先，让我们让此行为一直运行，直到出现故障。为此，我们将使用控制节点KeepRunningUntilFailure。

然后，我们将使用装饰器 GoalUpdater 来接受我们尝试遵循的动态对象姿势的更新。该节点将当前目标作为输入并订阅主题/goal_update。它设定了新目标 updated_goal，就好像收到了有关该主题的新目标一样。

为了与目标保持一定距离，我们将使用动作节点 .此节点修改路径，使其变短，因此我们不会尝试导航到感兴趣的对象。我们可以设置到目标的所需距离输入端口 .TruncatePathdistance

作为参考，此确切的行为树可供您使用包 nav2_bt_navigator 中包含的。

行为树如下：