Simple Commander API

Nav2 Simple Commander（Python 3）旨在为 Python 3 用户提供「把导航当作库」（navigation as a library）的能力。该 API 会为你处理所有 ROS 2 和 Action Server 相关的任务，让你能专注于构建基于 Nav2 能力的应用程序（前提是你已使用所选插件完成配置）。我们还提供了 API 使用演示和示例，展示了如何在 nav2_simple_commander 包中构建自主移动机器人领域的常见基础能力。

下面展示一个简单的示例。需要注意的是，goToPose()、goThroughPoses()、followWaypoints() 等方法是非阻塞的，这样你就可以在单线程应用中接收并处理反馈。因此，在等待任务完成时，需要使用 while not nav.isTaskComplete() 循环来轮询导航状态；如果任务尚未完成，可以顺便执行一些有用的工作（例如处理反馈、处理机器人正在采集的数据，或检查故障）。

你可以使用这个 Simple Commander 抢占（preempt）同类型的命令（例如，可以用另一个 goToPose() 抢占当前的 goToPose()），但如果要在 goToPose()、goThroughPoses() 或 followWaypoints() 之间切换，则必须显式取消当前命令并发出新命令。

  from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
  import rclpy

  rclpy.init()
  nav = BasicNavigator()

  # ...

  nav.setInitialPose(init_pose)
  nav.waitUntilNav2Active() # if autostarted, else use lifecycleStartup()

  # ...

  path = nav.getPath(init_pose, goal_pose)
  smoothed_path = nav.smoothPath(path)

  # ...

  nav.goToPose(goal_pose)
  while not nav.isTaskComplete():
    feedback = nav.getFeedback()
    if feedback.navigation_duration > 600:
      nav.cancelTask()

  # ...

  result = nav.getResult()
  if result == TaskResult.SUCCEEDED:
      print('Goal succeeded!')
  elif result == TaskResult.CANCELED:
      print('Goal was canceled!')
  elif result == TaskResult.FAILED:
      print('Goal failed!')

Commander API

Basic Navigator 提供的方法如下所示，包括输入参数和预期返回值。如果某个服务器请求失败，可能会抛出异常或返回 None 对象，因此请务必使用 try/catch 妥善包裹导航调用，并检查返回值是否为 None。

2023 年 9 月新增：Simple Navigator 构造函数将接受一个 namespace 字段，以支持多机器人应用或带命名空间的 Nav2 启动。

Robot Navigator 方法	描述
setInitialPose(initial_pose)	将机器人的初始位姿（PoseStamped）设置给定位模块。
goThroughPoses(poses, behavior_tree=”)	请求机器人穿过一组位姿（PoseStamped 列表）行驶。
goToPose(pose, behavior_tree=”)	请求机器人行驶到某个位姿（PoseStamped）。
followWaypoints(poses, number_of_loops=0, goal_index=0)	请求机器人跟随一组航点（PoseStamped 列表），从 goal_index（默认 0）开始，在第一遍之后重复 number_of_loops 次（默认 0，只运行一遍）。在每个位姿处将执行所选的 TaskExecutor 插件。
followPath(path, controller_id=”, goal_checker_id=”, progress_checker_id=”, path_handler_id=”)	请求机器人跟随一条从起点到终点位姿（PoseStamped）的路径（nav_msgs/Path）。
spin(spin_dist=1.57, time_allowance=10, disable_collision_checks=False)	请求机器人按给定角度原地旋转。
driveOnHeading(dist=0.15, speed=0.025, time_allowance=10, disable_collision_checks=False)	请求机器人按给定距离沿当前航向行驶。
backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False)	请求机器人按给定距离后退。
assistedTeleop(time_allowance=30)	请求机器人运行辅助遥操作（assisted teleop）动作。
cancelTask()	取消正在进行的任务，包括路线（route）任务。
isTaskComplete(task=RunningTask.NONE)	检查任务是否已完成，超时时间为 100ms。已完成返回 True，仍在进行返回 False。提供来自长时间运行任务（follow path、compute and track route 等）的任务 ID。
getFeedback(task=RunningTask.NONE)	获取任务的反馈，返回 action server 的反馈消息。提供你所请求任务的任务 ID。
getResult()	获取任务的最终结果，应在 isTaskComplete 返回 True 后调用。返回 action server 的结果消息。
getPath(start, goal, planner_id=”, use_start=False)	获取一条从起点到目标位姿（PoseStamped）的路径，nav_msgs/Path。
getPathThroughPoses(start, goals, planner_id=”, use_start=False)	获取一条从起点穿过一组目标位姿的路径，PoseStamped 列表，nav_msgs/Path。
getRoute(start, goal, use_start=False)	从给定的起点和终点 NodeID 或 PoseStamped 获取一条路线。如果给定了起点则使用起点，否则使用 TF 获取机器人位姿。
getAndTrackRoute(start, goal, use_start=False)	从给定的起点和终点 NodeID 或 PoseStamped 获取一条路线。如果给定了起点则使用起点，否则使用 TF 获取机器人位姿。
dockRobot(dock_pose, dock_type)	尝试将机器人在给定的停靠位姿和类型处停靠，不使用已知停靠点数据库。
dockRobot(dock_id)	尝试将机器人停靠在已知停靠点数据库中的给定停靠点 ID 处。
undockRobot(dock_type="")	取消停靠机器人。如果停靠时使用了 docking server 实例，则无需指定类型。
smoothPath(path, smoother_id=”, max_duration=2.0, check_for_collision=False)	平滑给定的 nav_msgs/Path 类型路径。
changeMap(map_filepath)	请求将当前地图切换为 map_filepath 对应的 yaml。
clearAllCostmaps()	同时清除全局和局部代价地图。
clearLocalCostmap()	清除局部代价地图。
clearGlobalCostmap()	清除全局代价地图。
clearLocalCostmapAroundPose(PoseStamped, distance)	清除给定位姿周围的局部代价地图。
clearGlobalCostmapAroundPose(PoseStamped, distance)	清除给定位姿周围的全局代价地图。
clearCostmapExceptRegion(distance)	清除除当前机器人位姿周围区域以外的代价地图。
getGlobalCostmap()	返回全局代价地图，nav2_msgs/Costmap。
getLocalCostmap()	返回局部代价地图，nav2_msgs/Costmap。
toggleCollisionMonitor(enable)	开启（True）或关闭（False）碰撞监控器。
followObjectByTopic(topic, max_duration=0)	请求机器人通过订阅给定话题来跟随一个物体。
followObjectByFrame(frame, max_duration=0)	请求机器人通过给定坐标系（frame）跟随一个物体。
waitUntilNav2Active(navigator=‘bt_navigator’, localizer=‘amcl’)	阻塞直到 Nav2 完全在线且生命周期节点处于激活（active）状态。与 autostart 或外部生命周期启动（bringup）配合使用。可以指定自定义的导航器和定位器节点。
lifecycleStartup()	向所有生命周期管理服务器发送请求，使它们进入激活状态；当 autostart 为 False 且希望由该程序控制 Nav2 的生命周期时使用。
lifecycleShutdown()	向所有生命周期管理服务器发送请求，使其关闭。
destroyNode()	释放该对象使用的资源。

Costmap API

这是 costmap 2d 消息的 Python 3 API，提供了 costmap 2d C++ API 中的基本坐标转换、get/set 以及处理语义。

Costmap 方法	描述
getSizeInCellsX()	获取以单元格为单位的 map 宽度。
getSizeInCellsY()	获取以单元格为单位的 map 高度。
getSizeInMetersX()	获取 x 轴方向的 map 尺寸（米）。
getSizeInMetersY()	获取 y 轴方向的 map 尺寸（米）。
getOriginX()	获取 map 的原点 x 轴 [m]。
getOriginY()	获取 map 的原点 y 轴 [m]。
getResolution()	获取 map 分辨率 [m/cell]。
getGlobalFrameID()	获取全局 frame_id。
getCostmapTimestamp()	获取代价地图时间戳。
getCostXY(mx, my)	使用地图坐标（Map Coordinate）的 mx（int）、my（int）获取代价地图中单元格的代价值（np.uint8）。
getCostIdx(index)	使用 index（int）获取代价地图中单元格的代价值（np.uint8）。
setCost(mx, my, cost)	使用地图坐标的 mx（int）、my（int）设置代价地图中单元格的代价值（np.uint8）。
mapToWorld(mx, my)	使用地图坐标 XY 的 mx（int）、my（int）获取世界坐标 XY 的 wx（float）[m]、wy（float）[m]。
worldToMapValidated(wx, wy)	使用世界坐标 XY 的 wx（float）[m]、wy（float）[m] 获取地图坐标 XY 的 mx（int）、my（int）。如果 wx、wy 坐标无效，返回 (None, None)。
getIndex(mx, my)	使用地图坐标 XY 的 mx（int）、my（int）获取单元格的索引（int）。

Footprint Collision Checker API

这是 Footprint Collision Checker（足迹碰撞检查器）的 Python 3 API。它提供了在给定地图中操作坐标和计算 Footprint 代价所需的方法。

Footprint Collision Checker 方法	描述
footprintCost(footprint)	检查足迹（Polygon）在其隐式提供的坐标位姿处是否发生碰撞。
lineCost(x0, x1, y0, y1, step_size=0.5)	遍历一条直线上的所有点并检查碰撞。该直线由 x0、y0、x1、y1、step_size（int）或（float）定义。
worldToMapValidated(wx, wy)	使用世界坐标 XY 的 wx（float）[m]、wy（float）[m] 获取地图坐标 XY 的 mx（int）、my（int）。如果 wx、wy 坐标无效，返回 (None, None)。如果尚未通过 setCostmap(costmap) 定义代价地图，则返回 None。
pointCost(x, y)	使用地图坐标 XY 获取代价地图中某点的代价值。（int）
setCostmap(costmap)	指定 footprint collision checker 使用哪个代价地图。（PyCostmap2D）
footprintCostAtPose(x, y, theta, footprint)	获取足迹在地图坐标中特定位姿处的代价值。x、y、theta（float），footprint（Polygon）。

示例与演示

所有这些都可以在包中找到。

nav2_simple_commander 提供了一些示例，用于展示可供用户使用的 API 函数：

• example_nav_to_pose.py — 演示导航器的导航到位姿（navigate to pose）能力，以及一些辅助方法。
• example_nav_through_poses.py — 演示导航器的穿过位姿导航（navigate through poses）能力，以及一些辅助方法。
• example_waypoint_follower.py — 演示导航器的航点跟随能力，以及一些辅助方法。
• example_follow_path.py — 演示导航器的路径跟随能力，以及一些辅助方法，如路径平滑。
• example_assisted_teleop.py — 演示导航器的辅助遥操作能力。
• example_route.py — 演示导航器的路线服务器（route server）能力。

nav2_simple_commander 还提供了一些演示，展示如何使用该 API 构建几种简单的自主应用：

• demo_security.py — 一个简单的安防机器人应用，展示如何使用 Navigate Through Poses 让机器人无限期地沿安防路线巡逻。
• demo_picking.py — 一个简单的取货应用，展示如何使用 Navigate To Pose 让机器人行驶到仓库中的特定货架，取一件物品或让人将物品放入货篮中，并将其送到发货目的地。
• demo_inspection.py — 一个简单的货架巡检应用，展示如何使用 Waypoint Follower 和任务执行器（task executor）对货架进行拍照、RFID 扫描等操作，以分析货架当前状态并在仓库中定位物品。