Hello Robot Stretch 3入门教程-x.演示＃1：地图和导航

Stretch 可以通过两种不同的方式感知和导航其周围环境：使用带有 ROS 2 导航堆栈的板载 2D 激光雷达，或使用带有 Stretch FUNMAP（快速统一导航、地图绘制和规划）的英特尔 RealSense 深度摄像头。以下教程将引导您了解启动和使用每个选项的基础知识。

Nav2 概述

ROS 2 导航堆栈（Nav2）是一个用于移动基座的运动规划和控制框架，可与 Stretch 直接配合使用。要使用 Nav2 堆栈，我们首先需要创建本地区域的地图，并使用游戏手柄遥控控制器在环境中移动机器人。完成后，Stretch 将能够在地图上进行定位，并自主在环境中导航。

构建地图

要启动构建地图演示，请打开终端窗口并运行以下命令：

ros2 launch stretch_nav2 offline_mapping.launch.py

ROS Visualizer (Rviz) 窗口将打开并显示正在构建的机器人和地图。

nav2地图形成

为了填充地图，我们需要移动机器人，使激光雷达能够覆盖您想要绘制的整个区域。您可以使用游戏手柄控制器来实现这一点。在此模式下，Dead man's Switch (LB)——按住此按钮，使用操纵杆控制机器人在环境中移动。

控制器按钮

键盘控制

ros2 launch stretch_nav2 offline_mapping.launch.py teleop_type:=keyboard

警告:这种建图方法会在激光雷达的高度创建一个二维地图，大约距离地面 6.5 英寸。后面的 ROS 2 教程将介绍如何通过结合激光雷达和头部摄像头的数据来创建三维地图。请记住，环境中的某些障碍物在此 2D 地图中不会显示出来。例如，机器人可能只能看到桌腿，这可能会导致机器人尝试穿过障碍物。如果没有障碍物突出超过 6.5 英寸（约 16.4 厘米），机器人也将无法看到不应尝试导航的区域 - 例如，地板上的 HVAC 通风口或向下的楼梯。为了安全起见，您应该考虑在创建地图时搭建临时屏障来阻挡这些区域。

 制图技巧

Ø 密切关注 Rviz 中显示的地图。深蓝绿色表示机器人未见过的区域。确保你希望机器人能够穿越的区域中间没有蓝绿色的“洞”。

Ø 机器人激光雷达的探测范围接近 360 度，但机器人桅杆会造成盲区。请务必不时命令机器人旋转，以便在导航过程中填补盲区。

Ø 导致 Stretch 轮子打滑的地面可能会导致地图错误。在此步骤中，请尽量避免在松散的地毯、厚绒地毯或高门槛等地面上操作。

 保存地图

一旦地图捕获了您希望 Stretch 能够导航的空间，请打开一个新的终端窗口并运行以下命令来保存地图：

ros2 run nav2_map_server map_saver_cli -f ${HELLO_FLEET_PATH}/maps/nav2_demo_map

这会将两个文件保存到“~/stretch_user/maps”目录 -nav2_demo_map.pgm和nav2_demo_map.yaml。 

提示:.pgm文件只是图像。您可以在图像查看器中打开之前保存的地图来查看它们。例如，尝试：

eog ${HELLO_FLEET_PATH}/maps/nav2_demo_map.pgm

导航

现在我们已经保存了环境地图，可以命令机器人在映射的空间内移动。运行以下命令：

ros2 launch stretch_nav2 navigation.launch.py map:=${HELLO_FLEET_PATH}/maps/nav2_demo_map.yaml

此时会打开一个 RViz 窗口。窗口左下方应该有一个标有“Startup”的按钮。按下此按钮将启动所有与导航相关的生命周期节点。

Rviz 现在会尝试在地图上显示机器人的位置；但是，最初显示的位置可能与机器人在真实空间中的位置不匹配。为了解决这个问题，请点击 Rviz 窗口顶部栏中的“2D 姿态估计”，然后在地图上大致点击机器人当前的当前位置，并沿机器人朝向的方向拖动。这将为定位包 (ACML) 提供机器人位置的初始估计值。

现在让我们命令机器人移动到另一个位置。点击顶部栏中的“2D Nav Goal”，然后点击另一个位置的地图。在终端窗口中，您将看到 Nav2 完成规划阶段，然后导航机器人到达目标。如果规划失败，机器人将执行恢复操作——原地旋转或后退。

nav2导航

FUNMAP 概述

使用 Stretch 进行地图绘制和导航的另一个选项是使用 FUNMAP 软件包。FUNMAP 代表“快速统一导航、地图绘制和规划”，它是一个软件包，旨在利用 Stretch 的一些独特元素，包括 Stretch 头部与眼睛高度相同的 D435if 深度摄像头。

要在 FUNMAP 中构建地图，首先要确保机器人位于一个空旷的区域，并且能够自由移动和旋转移动基座。首先在终端中运行以下命令：

ros2 launch stretch_funmap mapping.launch.py

其次，打开一个新的终端窗口或选项卡，并启动键盘操作节点：

ros2 run stretch_core keyboard_teleop --ros-args -p mapping_on:=True

Rviz 启动后，在同一个终端窗口中按下空格键即可启动头部扫描 - 命令机器人通过在房间内平移深度摄像头来创建地图，然后旋转移动底座并再次平移摄像头以克服桅杆造成的盲点。

此时，您应该在 RViz 中看到头部扫描生成的 3D 地图。您可以点击并拖动窗口来旋转并查看它。它是通过合并头部摄像头拍摄的多个 3D 扫描创建的。

现在，您可以为机器人指定导航目标。如果机器人找到通往目标的导航方案，它将尝试导航至该目标。导航过程中，机器人将使用深度摄像头来避开机器人前方的障碍物和动态障碍物。在 RViz 中，点击顶部栏上带有洋红色箭头图标的“2D 导航目标”按钮，然后通过点击并绘制洋红色箭头来指定地图地面上附近的导航目标位姿。要使此功能有效，导航目标必须位于机器人可以找到路径到达且机器人已充分扫描的位置。例如，机器人只会在其地图上存在的地面区域内导航。

如果机器人成功规划路径，它将在 RViz 中显示为连接白色球体的绿线，并且机器人将开始沿着该路径移动，直到到达您提供的目标。

现在让我们继续构建地图。当机器人到达新位置后，返回终端窗口并再次按下空格键，启动新的头部扫描。机器人将按照之前的流程进行操作，新位置的数据将添加到地图中。通过这种方式，您可以绘制障碍物、较大区域等周围的地图。您可以继续此过程，直到绘制完整个目标区域并在地图中清晰呈现。

保存地图

FUNMAP 地图是自上而下的图像，其中像素值代表在该位置观察到的最大 3D 点。地图生成后会自动保存 - 默认保存位置为：

~/stretch_user/debug/merged_maps/

您可以通过查看具有以下命名模式的文件来查看图像表示：

~/stretch_user/debug/merged_maps/merged_map_<DATETIME>_mhi_visualization.png