Turtlebot4入门教程-演示-导航

Turtlebot4入门教程-演示-导航

说明：

• 本教程将介绍使用 TurtleBot 4 和 Nav2 进行导航的各种方法。

相关设备：

• Turtlebot4机器人套件：采购地址

SLAM vs Localization

• 我们可以使用两种定位方法来确定机器人在地图上的位置：SLAM 或Localization。
• SLAM 允许我们在导航时生成地图，而Localization需要地图已经存在。

SLAM

• SLAM 对于生成新地图或在未知或动态环境中导航很有用。 它会在检测和更改时更新地图，但无法看到尚未发现的环境区域。

Localization

• 定位使用现有地图以及实时里程计和激光扫描数据来确定机器人在给定地图上的位置。 如果对环境进行了任何更改，它不会更新地图，但我们仍然可以在导航时避开新的障碍。 因为地图没有变化，我们可以获得更多可重复的导航结果。

• 在本教程中，我们将使用localization在 SLAM 生成的地图上导航。

Nav2

• TurtleBot 4 使用 Nav2 包进行导航。

启动导航

• 对于本教程，我们可以使用 Nav Bringup 启动导航。
• 在物理 TurtleBot 4 上，调用：

ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true map:=office.yaml

• 将 office.yaml 替换为您自己的地图。
• 如果您使用的是simulator，请调用：

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

• 这将在默认的 depot 世界中启动模拟，并将使用现有的 depot.yaml 文件作为地图。
• 如果您使用的是不同的世界，则需要为其创建地图并将其作为启动参数传递。
• 例如：

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true world:=classroom map:=classroom.yaml

与 Nav2 交互

• 在新终端中，启动 Rviz 以便您可以查看地图并与导航交互：

ros2 launch turtlebot4_viz view_robot.launch.py

• 效果图

• Rviz 窗口的顶部是工具栏。 您会注意到有三种导航工具可供您使用。

• 导航工具

2D Pose Estimate

• 2D Pose Estimate 工具用于定位，以设置机器人在地图上的近似初始位姿。
• Nav2 堆栈需要这样做才能知道从哪里开始本地化。
• 单击该工具，然后单击并拖动地图上的箭头以大致确定机器人的位置和方向。
• 效果图

Publish Point

• 发布点工具允许您单击地图上的一个点，并将该点的坐标发布到 /clicked_point 主题。
• 打开一个新终端并调用：

ros2 topic echo /clicked_point

• 然后，选择发布点工具并单击地图上的一个点。 您应该会在终端中看到发布的坐标。

Nav2 Goal

• Nav2 目标工具允许您为机器人设置目标姿势。

• 然后，Nav2 堆栈将规划一条通往目标姿势的路径，并尝试将机器人驱动到那里。

• 确保在设置目标姿势之前设置机器人的初始姿势。

• 效果图

TurtleBot 4 Navigator

• TurtleBot 4 Navigator 是添加到 Nav2 Simple Commander 的 Python 节点。
• 它包括 TurtleBot 4 的特定功能，例如停靠和取消停靠，以及易于使用的导航方法。
• TurtleBot 4 Navigator 至少需要 Nav2 Simple Commander 1.0.11 版本
• 以下示例的代码可在 https://github.com/turtlebot/turtlebot4_tutorials 获得。
• 对于每个示例，机器人从地图原点的码头开始。

Navigate to Pose

• 此示例演示了与 Nav2 目标相同的行为。 Nav2 堆栈在地图上给出了一个姿势，它用它计算路径。
• 然后机器人尝试沿着路径行驶。 此示例在 TurtleBot 4 模拟的仓库世界中进行了演示。
• 要运行此示例，请启动 Ignition 模拟：

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

• 模拟开始后，打开另一个终端并运行：

ros2 run turtlebot4_python_tutorials nav_to_pose

代码分解

• 此示例的源代码可在此处获得。
• 让我们看一下主要功能。

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Set goal poses
    goal_pose = navigator.getPoseStamped([13.0, 5.0], TurtleBot4Directions.EAST)

    # Undock
    navigator.undock()

    # Go to each goal pose
    navigator.startToPose(goal_pose)

    rclpy.shutdown()

初始化节点

• 我们首先初始化 rclpy 并创建 TurtleBot4Navigator 对象。
• 这将初始化我们需要的任何 ROS2 发布者、订阅者和操作客户端。

rclpy.init()

navigator = TurtleBot4Navigator()

对接机器人

• 接下来，我们检查机器人是否停靠。 如果不是，我们发送一个动作目标来对接机器人。
• 通过对接机器人，我们保证它在地图上的 [0.0, 0.0] 坐标处。

if not navigator.getDockedStatus():
    navigator.info('Docking before intialising pose')
    navigator.dock()

设置初始姿势

• 现在我们知道机器人已停靠，我们可以将初始位姿设置为 [0.0, 0.0]，面向“北”。

initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
navigator.setInitialPose(initial_pose)

• TurtleBot 4 Navigator 使用基本方向来设置机器人相对于地图的方向。
• 如果需要更精确的方向，可以使用实际整数或浮点数。

class TurtleBot4Directions(IntEnum):
    NORTH = 0
    NORTH_WEST = 45
    WEST = 90
    SOUTH_WEST = 135
    SOUTH = 180
    SOUTH_EAST = 225
    EAST = 270
    NORTH_EAST = 315

• 这些基本方向是相对于地图的，而不是实际的磁北极。
• 向北行驶相当于在地图上向上行驶，向西行驶是向左行驶，以此类推。

Wait for Nav2

• 设置初始位置后，Nav2 堆栈会将机器人放置在地图上的该位置并开始定位。
• 在我们开始发送导航目标之前，我们希望等待 Nav2 准备好。

navigator.waitUntilNav2Active()

• 此调用将阻塞，直到 Nav2 准备好。 确保您已在单独的终端中启动导航显示。

设定目标姿势

• 现在我们可以创建一个 geometry_msgs/PoseStamped 消息。
• getPoseStamped 方法对我们来说很容易。
• 我们所要做的就是传入一个列表，该列表描述了我们想要在地图上行驶到的 x 和 y 位置，以及我们希望机器人到达该点时所面对的方向。

goal_pose = navigator.getPoseStamped([13.0, 5.0], TurtleBot4Directions.EAST)

解开机器人并进入目标姿势

• 我们准备开车到目标姿势。 我们首先断开机器人的对接，这样它就不会尝试开车穿过码头，然后发送目标姿势。
• 当机器人驱动到目标姿势时，我们将收到来自动作的反馈。 该反馈包括预计到达时间。

navigator.undock()
navigator.startToPose(goal_pose)

• 一旦机器人达到目标，我们调用 rclpy.shutdown() 优雅地销毁 rclpy 上下文。

在 Rviz 中观看导航进度

• 您可以通过调用在 Rviz 中可视化导航过程：

ros2 launch turtlebot4_viz view_robot.launch.py

• 效果图

Navigate Through Poses

• 此示例演示 Navigate Through Poses 行为树。 Nav2 堆栈在地图上给出一组姿势，并创建一条路径，依次通过每个姿势，直到到达最后一个姿势。 然后机器人尝试沿着路径行驶。 此示例在 TurtleBot 4 模拟的仓库世界中进行了演示。

• 要运行此示例，请启动 Ignition 模拟：

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

• 模拟开始后，打开另一个终端并运行：

ros2 run turtlebot4_python_tutorials nav_through_poses

代码分解

• 此示例的源代码可在此处获得。

• 让我们看一下主要功能。

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Set goal poses
    goal_pose = []
    goal_pose.append(navigator.getPoseStamped([0.0, -1.0], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([1.7, -1.0], TurtleBot4Directions.EAST))
    goal_pose.append(navigator.getPoseStamped([1.6, -3.5], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([6.75, -3.46], TurtleBot4Directions.NORTH_WEST))
    goal_pose.append(navigator.getPoseStamped([7.4, -1.0], TurtleBot4Directions.SOUTH))
    goal_pose.append(navigator.getPoseStamped([-1.0, -1.0], TurtleBot4Directions.WEST))

    # Undock
    navigator.undock()

    # Navigate through poses
    navigator.startThroughPoses(goal_pose)

    # Finished navigating, dock
    navigator.dock()

    rclpy.shutdown()

• 此示例的开头与导航到姿势相同。 我们初始化节点，确保机器人已停靠，并设置初始位姿。 然后我们等待 Nav2 激活。

设定目标姿势

• 下一步是创建一个 PoseStamped 消息列表，这些消息代表机器人需要通过的姿势。

goal_pose = []
goal_pose.append(navigator.getPoseStamped([0.0, -1.0], TurtleBot4Directions.NORTH))
goal_pose.append(navigator.getPoseStamped([1.7, -1.0], TurtleBot4Directions.EAST))
goal_pose.append(navigator.getPoseStamped([1.6, -3.5], TurtleBot4Directions.NORTH))
goal_pose.append(navigator.getPoseStamped([6.75, -3.46], TurtleBot4Directions.NORTH_WEST))
goal_pose.append(navigator.getPoseStamped([7.4, -1.0], TurtleBot4Directions.SOUTH))
goal_pose.append(navigator.getPoseStamped([-1.0, -1.0], TurtleBot4Directions.WEST))

Navigate through the poses

• 现在我们可以解除机器人的停靠并开始通过每个点导航。 一旦机器人达到最终姿势，它将返回码头。

navigator.undock()

navigator.startThroughPoses(goal_pose)

navigator.dock()

• 在 Rviz 中观看导航进度
• 您可以通过调用在 Rviz 中可视化导航过程：

ros2 launch turtlebot4_viz view_robot.launch.py

跟随航点

• 此示例演示如何跟踪航路点。 Nav2 堆栈在地图上被赋予一组航点，并创建一条按顺序通过每个航点的路径，直到到达最后一个航点。 然后机器人尝试沿着路径行驶。 此示例与通过姿势导航之间的区别在于，当跟随航路点时，机器人将计划单独到达每个航路点，而不是计划通过驾驶通过其他姿势到达最后一个姿势。 此示例在 TurtleBot 4 模拟的仓库世界中进行了演示。

• 要运行此示例，请启动 Ignition 模拟：

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

• 模拟开始后，打开另一个终端并运行：

ros2 run turtlebot4_python_tutorials follow_waypoints

代码分解

• 此示例的源代码可在此处获得。
• 让我们看一下主要功能。

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Set goal poses
    goal_pose = []
    goal_pose.append(navigator.getPoseStamped([-3.3, 5.9], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([2.1, 6.3], TurtleBot4Directions.EAST))
    goal_pose.append(navigator.getPoseStamped([2.0, 1.0], TurtleBot4Directions.SOUTH))
    goal_pose.append(navigator.getPoseStamped([-1.0, 0.0], TurtleBot4Directions.NORTH))

    # Undock
    navigator.undock()

    # Follow Waypoints
    navigator.startFollowWaypoints(goal_pose)

    # Finished navigating, dock
    navigator.dock()

    rclpy.shutdown()

• 此示例与 Navigate Through Poses 非常相似。 不同之处在于我们使用不同的姿势作为我们的航点，并且我们使用 startFollowWaypoints 方法来执行我们的导航行为。

在 Rviz 中观看导航进度

• 您可以通过调用在 Rviz 中可视化导航过程：

ros2 launch turtlebot4_viz view_robot.launch.py

• 效果图

创建路径

• 此示例演示如何在运行时在 Rviz 中创建导航路径。 它使用 2D Pose Estimate 工具向 TurtleBot 4 Navigator 传递一组姿势。 然后我们使用 Follow Waypoints 行为来跟随这些姿势。 此示例在物理 TurtleBot 4 上运行。

• 要运行此示例，请在您的 PC 或 Raspberry Pi 上启动导航启动：

ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true map:=office.yaml

• 将 office.yaml 替换为您的环境地图。

• 导航开始后，打开另一个终端并运行：

ros2 run turtlebot4_python_tutorials create_path

• 在您的 PC 上，您需要启动 Rviz：

ros2 launch turtlebot4_viz view_robot.launch.py

代码分解

• 此示例的源代码可在此处获得。
• 让我们看一下主要功能。

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Set goal poses
    goal_pose = navigator.createPath()

    if len(goal_pose) == 0:
        navigator.error('No poses were given, exiting.')
        exit(0)

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.clearAllCostmaps()
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Undock
    navigator.undock()

    # Navigate through poses
    navigator.startFollowWaypoints(goal_pose)

    # Finished navigating, dock
    navigator.dock()

    rclpy.shutdown()

• 这个例子和其他例子一样开始初始化 TurtleBot 4 Navigator。

Create your path

• 初始化后，系统会提示用户使用 2D Pose Estimate 工具创建他们的路径。
• 您必须设置至少一个姿势。 设置完所有姿势后，用户可以按 CTRL + C 停止创建路径并开始导航。

goal_pose = navigator.createPath()

if len(goal_pose) == 0:
    navigator.error('No poses were given, exiting.')
    exit(0)

设置初始姿势并清除成本图

• 接下来我们设置初始姿势并清除所有成本图。 我们清除成本图是因为 2D Pose Estimate 工具已被 Nav2 堆栈订阅，并且每次我们使用它时，Nav2 都会假设机器人处于该位置，而实际上并非如此。 清除成本图将消除创建路径时可能产生的任何错误成本图。

if not navigator.getDockedStatus():
    navigator.info('Docking before intialising pose')
    navigator.dock()

initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
navigator.clearAllCostmaps()
navigator.setInitialPose(initial_pose)

navigator.waitUntilNav2Active()

• 在继续之前，我们还等待 Nav2 处于活动状态。

跟随路径

• 现在我们可以取消停靠并遵循创建的路径。 在此示例中，我们使用跟随航点行为，但这可以很容易地替换为导航通过姿势。

navigator.undock()

navigator.startFollowWaypoints(goal_pose)

navigator.dock()

• 我们通过对接机器人来完成示例。 这假设创建路径中的最后一个姿势在停靠点附近。 如果不是，您可以删除此操作。

使用 Rviz 创建路径

• 运行此示例将如下所示：
• 效果图

• 创建路径后，您将看到机器人被放置在您单击的位置。 这是正常的，当 TurtleBot 4 Navigator 设置初始姿势时会被清除。