260515

继我们上次的进度，我们的整个结构可以理解为：

nav2_params.yaml
├── bt_navigator        # 接收 NavigateToPose action，管理导航行为树
├── controller_server   # 跟踪路径，输出速度
├── planner_server      # 根据地图规划全局路径
├── behavior_server     # 处理恢复行为，比如旋转、后退、等待
├── local_costmap       # 机器人附近的局部代价地图
└── global_costmap      # 全局地图上的代价地图

这里我们接触到了一个新概念——代价地图。

这个概念的用途是什么呢？

要知道，我们地图上的点是没有体积的，而我们的小车是有物理宽度的，如果让导航算法让小车的中心点紧贴着 SLAM 的黑线走，那小车很可能就会卡进墙壁里，或者直接撞墙。

所以代价地图存在的目的，就是解决机器人的物理体积和动态躲避的问题，它可以将我们黑白的二维地图，给小车翻译为 0~254 危险指数的危险指数“等高图”。

可以这么认为：

0(空旷区)：随便开，非常安全。
1~252(膨胀危险区)：离障碍物有点近了，虽然不会撞上，但最好是绕着走。
253(内切区)：非常危险，此时机器人肯定已经至少蹭到障碍物了。
254(致命障碍)：绝对的障碍物，必然已经撞上去了。

这部分是对于代价地图的理解。

然后就是 bt_navigator 了，这部分是管理小车的行为树的，不多做解释。

controller_server则是根据当前的坐标系位置、当前速度、planner_server 给出的 path 等，在 path 上找一个前视点(在当前规划路径上选一个距离机器人不远的一个临时跟踪点)，最后根据当前位置和前视点来计算所需的线速度、角速度，同时用 local costmap 检查附近障碍物和碰撞风险。

behavior_server 和 controller_server 虽然都是控制机器人具体行为的，但是两者的功能上是不一样的。

behavior_server 用于处理需要恢复的行为，比如失败、卡住或需要从不可导航到可导航等动作的时候，就由它来负责后退、旋转等这些短动作。

如果恢复动作都失败了，nav2 就会返回失败。

总的来说，整个链路为：

flowchart LR goal["目标点"] --> bt["bt_navigator"] bt --> planner["planner_server"] planner --> global["global_costmap"] planner --> path["全局路径"] path --> controller["controller_server"] controller --> local["local_costmap"] controller --> raw["/cmd_vel_raw"] raw --> watchdog["watchdog"] watchdog --> cmd["/cmd_vel"] cmd --> robot["机器人"] controller -->|"失败/卡住"| bt bt --> behavior["behavior_server"] behavior --> raw behavior -->|"恢复后"| planner

即，bt_navigator 为调度者，planner_server负责怎么从当前位置到目标点规划出一条路，global_costmap 是给 planner 看全局哪里可以走，controller_server负责沿着这条路实际的开过去，local_costmap 给机器人看看附近有没有障碍，behavior_server 只在失败或恢复分支里执行短动作。

这就是我们 nav2 的 config 文件的写法了。

接下来，我们要写一个启动文件来启动 nav2，我们可以在 src/kibot_one_sim/launch/nav2.launch.py 中写入如下内容：

# mypy: disable-error-code="import-untyped"

from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description() -> LaunchDescription:
    sim_pkg_share = Path(get_package_share_directory("kibot_one_sim"))
    default_params_file = sim_pkg_share / "config" / "nav2_params.yaml"

    use_sim_time_arg = DeclareLaunchArgument(
        name="use_sim_time",
        default_value="true",
        description="Use Gazebo /clock.",
    )
    params_file_arg = DeclareLaunchArgument(
        name="params_file",
        default_value=str(default_params_file),
        description="Nav2 parameters file.",
    )
    autostart_arg = DeclareLaunchArgument(
        name="autostart",
        default_value="true",
        description="Automatically activate Nav2 lifecycle nodes.",
    )

    use_sim_time = LaunchConfiguration("use_sim_time")
    params_file = LaunchConfiguration("params_file")
    autostart = LaunchConfiguration("autostart")

    common_params = [params_file, {"use_sim_time": use_sim_time}]
    common_remappings = [
        ("/tf", "tf"),
        ("/tf_static", "tf_static"),
    ]
    velocity_remappings = common_remappings + [
        ("/cmd_vel", "/cmd_vel_raw"),
        ("cmd_vel", "/cmd_vel_raw"),
    ]

    controller_server = Node(
        package="nav2_controller",
        executable="controller_server",
        name="controller_server",
        output="screen",
        parameters=common_params,
        remappings=velocity_remappings,
    )

    planner_server = Node(
        package="nav2_planner",
        executable="planner_server",
        name="planner_server",
        output="screen",
        parameters=common_params,
        remappings=common_remappings,
    )

    behavior_server = Node(
        package="nav2_behaviors",
        executable="behavior_server",
        name="behavior_server",
        output="screen",
        parameters=common_params,
        remappings=velocity_remappings,
    )

    bt_navigator = Node(
        package="nav2_bt_navigator",
        executable="bt_navigator",
        name="bt_navigator",
        output="screen",
        parameters=common_params,
        remappings=common_remappings,
    )

    lifecycle_manager = Node(
        package="nav2_lifecycle_manager",
        executable="lifecycle_manager",
        name="lifecycle_manager_navigation",
        output="screen",
        parameters=[
            {
                "use_sim_time": use_sim_time,
                "autostart": autostart,
                "node_names": [
                    "controller_server",
                    "planner_server",
                    "behavior_server",
                    "bt_navigator",
                ],
            }
        ],
    )

    return LaunchDescription(
        [
            use_sim_time_arg,
            params_file_arg,
            autostart_arg,
            controller_server,
            planner_server,
            behavior_server,
            bt_navigator,
            lifecycle_manager,
        ]
    )

这里主要是配置文件，包括参数配置等，以及 lifecycle 等节点，我们不做多的解释了。

我们重点关注的是下面的内容，主要是 remapping 这个东西，重映射，和我们 ROS 和 Gazebo 的 bridge 的重映射不同的是，bridge 的重映射是将 Gazebo 和我们的 ROS 进行连接，干的是将 Gazebo/ROS 的话题进行连接起来。

而我们这里的启动文件则是将 Nav2 的默认配置的话题映射到我们所指定的话题，比如这里我们就将速度发布的话题从 cmd_vel 给映射到了 cmd_vel_raw 。

然后我们安装一下所需的依赖：

sudo apt install ros-jazzy-navigation2 ros-jazzy-nav2-bringup

KiSpace

【Gazebo 学习】260515

260515

Jese__Ki

260515

【Gazebo 学习】260516

【Gazebo 学习】260514

Jese__Ki