ROS2入门：导航入门
一、前言
ROS2（Robot Operating System 2）是机器人开发的重要中间件，其中导航（Navigation）模块是实现自主移动机器人的核心部分。本部分将带你快速入门 ROS2 Navigation2（Nav2），理解其基本概念，并通过仿真完成第一个导航案例。
二、导航基本概念
导航（Navigation）的核心目标：让机器人能够从当前位置 自主移动 到指定目标点，同时避开障碍物。
典型的导航系统包含以下几个功能模块：
- 定位（Localization）：确定机器人在地图中的位置。
- 建图（Mapping）：在未知环境中创建地图。
- 路径规划（Path Planning）：计算从起点到目标点的可行路径。
- 运动控制（Control）：生成底盘的速度指令，驱动机器人执行。
下面我们按照这个流程来尝试完成一次全流程的导航。从建立一张地图开始。
这里我们准备一个全新的工作空间，并将上一部分的机器人仿真的包fishbot_description放入新的工作空间，以免弄乱之前章节的代码。
准备一个合适的世界来，替换包下面之前建立的简陋的世界，这里可以直接复制 鱼香ROS世界示例。新的世界地图大概长这样：
三、使用 slam-toolbox 建立地图
在真实或仿真环境中，通常需要先建立地图。ROS2 提供了 slam-toolbox 作为主流的 SLAM 方案。 slam-toolbox 主要用于构建二维地图，对应单线/平面的激光雷达。
1. 安装 slam-toolbox：
sudo apt install ros-$ROS_DISTRO-slam-toolbox2. 启动 SLAM 节点：
ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True如下： 
3. 打开 RViz2进行建图
在界面中通过键盘或话题控制机器人移动，它会边走边建图。（需要添加地图组件） 这里要注意，建图时一定要让小车的速度慢一点，防止里程计漂移。
4. 保存地图：
使用工具保存地图
安装工具nav2-map-server：
sudo apt install ros-$ROS_DISTRO-nav2-map-server我们新建一个功能包，将当前的地图保存到目录里，进行后续导航测试：
ros2 pkg create fishbot_navigation2并且在功能包下新建maps文件夹，在该文件夹路径下使用命令保存地图：
ros2 run nav2_map_server map_saver_cli -f room这样，我们就得到了环境的地图文件（map.yaml 和 map.pgm）。
有了这个地图，我们就可以进一步去实践机器人导航了。
四、Navigation2 入门
1.Navigation2介绍
Navigation2（Nav2）是 ROS2 官方提供的导航框架，主要特点：
- 模块化设计（可替换算法插件）。
- 支持 SLAM、AMCL 定位、DWB 控制器等。
- 支持仿真环境（Gazebo / RViz2）。
主要组件：
- amcl：自适应蒙特卡洛定位算法。
- map_server：地图加载与提供服务。
- planner_server：全局路径规划。
- controller_server：局部控制器。
- recoveries_server：故障恢复行为（原地旋转等）。
2.Navigation2环境搭建
安装 Navigation2：
sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup测试 RViz2 是否能运行：
rviz2配置Navigation2参数，在功能包下新建config文件夹，并将Navigation2的默认参数复制过来，默认参数文件位于：cat /opt/ros/$ROS_DISTRO/share/nav2_bringup/params/nav2_params.yaml
修改机器人半径为0.12：
robot_radius: 0.123.编写launch并启动导航
新建launch文件：src/fishbot_navigation2/launch/navigation2.launch.py
import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
# 获取与拼接默认路径
fishbot_navigation2_dir = get_package_share_directory(
'fishbot_navigation2')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
rviz_config_dir = os.path.join(
nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
# 创建 Launch 配置
use_sim_time = launch.substitutions.LaunchConfiguration(
'use_sim_time', default='true')
map_yaml_path = launch.substitutions.LaunchConfiguration(
'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml'))
nav2_param_path = launch.substitutions.LaunchConfiguration(
'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))
return launch.LaunchDescription([
# 声明新的 Launch 参数
launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
description='Use simulation (Gazebo) clock if true'),
launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path,
description='Full path to map file to load'),
launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path,
description='Full path to param file to load'),
launch.actions.IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
# 使用 Launch 参数替换原有参数
launch_arguments={
'map': map_yaml_path,
'use_sim_time': use_sim_time,
'params_file': nav2_param_path}.items(),
),
launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'),
])这个 launch 文件的作用是启动 FishBot 的导航栈（Navigation2）,主要干了下面几件事情：
- 定义参数：声明 use_sim_time、map 和 params_file，分别用于仿真时间、地图文件和导航参数文件。
- 启动 Nav2：通过 IncludeLaunchDescription 调用 nav2_bringup 的 bringup_launch.py，并把上述参数传给它，实现导航核心功能。
- 启动 RViz 可视化：加载默认 RViz 配置文件，用于观察机器人在地图上的位置、路径规划和传感器信息。
在CmakeLists.txt里编写命令，拷贝这些文件：
install(DIRECTORY config launch maps
DESTINATION share/${PROJECT_NAME}
)编译：
colcon build
source install/setup.bash启动仿真：
ros2 launch fishbot_description gazebo_sim.launch.py启动导航：
ros2 launch fishbot_navigation2 navigation2.launch.py在打开的rviz2里设定机器人初始位置和朝向（大概值即可）
紫色区域 → 全局代价地图（用于全局路径规划） 红色区域 → 局部代价地图（用于动态避障和局部修正）
4.单点与路点导航
使用Nav2 Goal 指定一个目标点和朝向，机器人就会自动前往：
也可以使用Waypoint功能一次指定多个点，机器人会依次经过这些点：
5.常用参数调优
优化导航速度和膨胀半径
在默认配置下，机器人可能移动得比较慢，且避障范围过大。可以通过修改fishbot_navigation2/config/nav2_params.yaml 来优化：
调整机器人速度: 在 controller_server 部分，修改 FollowPath 控制器的速度限制：
controller_server:
ros__parameters:
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
# 最大速度和加速度
max_vel_x: 0.4 # 最大前进速度 (m/s)，可根据需求调大，如 0.6
min_vel_x: 0.0
max_vel_theta: 1.0 # 最大旋转速度 (rad/s)，默认偏小可调到 1.5
min_vel_theta: 0.2
acc_lim_x: 0.5 # 线速度加速度限制
acc_lim_theta: 1.5 # 角速度加速度限制增大 max_vel_x 和 max_vel_theta 能让机器人跑得更快，调大 acc_lim_x 和 acc_lim_theta 则能让加速更灵活。
调整膨胀半径：
在 global_costmap 与 local_costmap 部分，找到 inflation_layer 配置：
global_costmap:
global_costmap:
ros__parameters:
inflation_layer:
inflation_radius: 0.3 # 默认一般是 0.5，可以缩小到 0.3
local_costmap:
local_costmap:
ros__parameters:
inflation_layer:
inflation_radius: 0.3 # 局部避障范围也缩小inflation_radius 越大，机器人会更保守，离障碍物更远；缩小数值后，机器人能更贴近障碍物通过，但要注意安全。
左图改小inflation_radius 就会变成右图的效果：
优化机器人到点精度
机器人到达目标点时，通常会有一个位置容差和角度容差参数，用来决定什么时候算“到点”。
xy_goal_tolerance：表示机器人位置到目标点的允许误差（单位：米）。 数值越小，机器人需要更精确地停在目标点上，但可能会反复调整甚至原地抖动。 数值稍大，机器人更容易判定到点，但精度会降低。
yaw_goal_tolerance：表示机器人朝向角度的允许误差（单位：弧度）。 数值越小，机器人必须严格朝向目标方向，可能会在原地不断转动。 数值稍大，可以减少调整时间，但最终朝向可能不完全对准。
latch_xy_goal_tolerance：是否锁定位置容差。启用后，只要机器人进入目标区域，就不再因为小幅偏离而重新调整。
这部分参数在 nav2_params.yaml 文件中的 bt_navigator 模块可以找到：
bt_navigator:
ros__parameters:
# 到点位置容差 (米)
xy_goal_tolerance: 0.05
# 到点角度容差 (弧度)
yaw_goal_tolerance: 0.1
# 是否锁定位置容差
latch_xy_goal_tolerance: false五、Navigation2 应用
在完成了前面的环境配置和参数调优后，我们可以通过多种方式来调用 Navigation2 功能，实现机器人导航。以下是常用的几种方式：
1.使用话题初始化机器人位姿
当启动 RViz2 时，需要告诉 Nav2 机器人在地图中的初始位置。 这可以通过 RViz2 中的 “2D Pose Estimate” 工具手动设置，也可以通过话题发布来完成：
ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
header:
frame_id: 'map'
pose:
pose:
position: {x: 0.0, y: 0.0, z: 0.0}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}"上面命令会把机器人初始位姿设在地图原点。我们也可以使用python或者C++来实现这样的功能：
我们通过新建功能包 fishbot_application，写一个节点往/initialpose发布消息，效果等同于 RViz2 里的 2D Pose Estimate。主要使用Nav2 的 Python 简化接口（BasicNavigator）。
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python fishbot_application --dependencies rclpy geometry_msgs在功能包目录下新建文件fishbot_application/fishbot_application/initial_pose_node.py：
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy
def main():
rclpy.init()
navigator = BasicNavigator()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 0.0
initial_pose.pose.position.y = 0.0
initial_pose.pose.orientation.w = 1.0
navigator.setInitialPose(initial_pose)
navigator.waitUntilNav2Active()
rclpy.spin(navigator)
rclpy.shutdown()
if __name__ == '__main__':
main()在fishbot_application/setup.py中找到 entry_points，改成：
entry_points={
'console_scripts': [
'initial_pose_node = fishbot_application.initial_pose_node:main',
],
},编译并运行
colcon build
source install/setup.bash
ros2 run fishbot_application initial_pose_node运行后会往 /initialpose 发布一次初始位姿。
2.使用TF获取机器人实时位置
3.调用接口完成单点导航
除了在 RViz2 里用 “Nav2 Goal” 工具点击目标点，也可以通过 Action 接口调用：
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "
pose:
header:
frame_id: 'map'
pose:
position: {x: 1.0, y: 2.0, z: 0.0}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}"上面命令会让机器人导航到 (1.0, 2.0) 位置。
4.调用接口完成路点导航
Navigation2 也支持一次性发送多个目标点（Waypoint）。
ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints "
poses:
- header:
frame_id: 'map'
pose:
position: {x: 1.0, y: 1.0, z: 0.0}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
- header:
frame_id: 'map'
pose:
position: {x: 2.0, y: 2.0, z: 0.0}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}"