5、导航

SLAM vs Localization

本教程将介绍使用TurtleBot4机器人和Nav2导航的各种方法。

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

SLAM

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

Localization

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

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

Nav2

TurtleBot 4机器人使用Nav2堆栈进行导航。

启动导航

• GALACTIC 

• HUMBLE 

• 对于本教程，我们可以使用 Nav Bringup启动导航。

  在实体TurtleBot 4机器人上，运行：

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

  将 office.yaml替换为您自己的地图。

  如果您正在使用仿真，请运行：

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

  这将在默认的 depot世界中启动模拟，并将使用现有的depot.yaml文件作为地图。如果你使用的是一个不同的世界，你需要为它创建一个地图，并将其作为启动参数传入。

  For example:

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

 

与Nav2交互

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

ros2 launch turtlebot4_viz view_robot.launch.py

At the top of the Rviz window is the toolbar. You will notice that there are three navigation tools available to you.

TurtleBot 4 Navigator

TurtleBot 4 Navigator是添加到Nav2 Simple Commander的Python节点。它包括TurtleBot 4的特定功能，如对接和脱离对接，以及易于使用的导航方法。

导航到姿势

此示例演示了与 Nav2 Goal相同的行为。Nav2堆栈在地图上给定一个姿势，用于计算路径。然后机器人试图沿着路径行驶。该示例在TurtleBot 4仿真的 depot世界中进行了演示。

要运行此示例，请启动点火模拟：

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

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

ros2 run turtlebot4_python_tutorials nav_to_pose

代码分解

此处提供了此示例的源代码 here。

让我们来看看主要功能

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对象。这将初始化我们需要的任何ROS 2发布者、订阅者和动作客户端。

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导航器使用基本方向来设置机器人相对于地图的方向。如果需要更精确的方向，可以使用实际整数或浮点。

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

等待Nav2

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

navigator.waitUntilNav2Active()

 

设定目标姿势

现在我们可以创建一个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”行为树。Nav2堆栈在地图上提供一组姿势，并创建一条路径，该路径按顺序穿过每个姿势，直到达到最后一个姿势。然后机器人试图沿着路径行驶。该示例在TurtleBot 4仿真的 depot世界中进行了演示。

要运行此示例，请启动点火模拟：

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

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

ros2 run turtlebot4_python_tutorials nav_through_poses

代码分解

此处提供了此示例的源代码here.

让我们来看看主要功能。

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()

此示例的开始与navigate to pose相同。我们初始化节点，确保机器人对接，并设置初始姿势。然后我们等待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))

浏览姿势

现在我们可以解开机器人的对接，开始在每个点上导航。一旦机器人达到最终姿势，它将返回码头。

navigator.undock() navigator.startThroughPoses(goal_pose) navigator.dock()

在Rviz中观看导航进度

您可以通过调用以下命令来可视化Rviz中的导航过程：

ros2 launch turtlebot4_viz view_robot.launch.py

跟随航路点

此示例演示了如何遵循路点。Nav2堆栈在地图上提供一组航点，并创建一条按顺序穿过每个航点的路径，直到到达最后一个航点。然后机器人试图沿着路径行驶。该示例与“通过姿势导航”之间的区别在于，当跟随航路点时，机器人将计划单独到达每个航路点，而不是计划通过其他姿势到达最后一个姿势。该示例在TurtleBot 4仿真的 depot世界中进行了演示。

要运行此示例，请启动点火模拟：

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

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

ros2 run turtlebot4_python_tutorials follow_waypoints

代码分解

此处提供了此示例的源代码here.

让我们来看看主要功能。

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上启动Navigation。

导航启动后，打开另一个终端并运行：

ros2 run turtlebot4_python_tutorials create_path

在您的电脑上，您需要启动Rviz：

ros2 launch turtlebot4_viz view_robot.launch.py

代码分解

此处提供了此示例的源代码 here.

让我们来看看主要功能。

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开始。

创建您的路径

初始化后，提示用户使用 2D Pose Estimate工具创建路径。您必须至少设置一个姿势。一旦所有的姿势都设置好了，机器人就会开始导航。

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

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

接下来，我们设置初始姿势并清除所有成本图。我们之所以清除成本图，是因为Nav2堆栈订阅了2D姿势估计工具，并且每次我们使用它时，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激活后再继续。

沿着这条路走

现在我们可以解除对接并按照创建的路径进行操作。在本例中，我们使用了“Follow Waypoints”行为，但这可以很容易地替换为Navigate Through Poses。

navigator.undock() navigator.startFollowWaypoints(goal_pose) navigator.dock()

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

使用Rviz创建路径

运行此示例的情况如下：