# 3. 编写 Launch 并启动导航

有了参数，接着我们来编写一个 Launch 文件来传递参数并启动导航。

在编写 launch 前，需要将slam部分建立好的地图保存拷贝一份到 src/fishbot_navigation2/maps 文件夹下。

## 一、编写Launch

在 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='false')
    map_yaml_path = launch.substitutions.LaunchConfiguration(
        'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'fishbot_map.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 对外提供三个可配置的参数，是否使用仿真时间 use_sim_time，地图文件路径 map_yaml_path 和导航参数路径 nav2_param_path，默认值都已经设好了。

接着修改 CMakeLists.txt，添加 launch、config  和 maps 三个目安装到 install 目录下的指令，然后重新构建功能包完成文件拷贝。

## 二、启动底盘和雷达

必要步骤，不再赘述

## 三、启动导航

```
colcon build
source install/setup.bash
ros2 launch fishbot_navigation2 navigation2.launch.py
```

启动后可以看到 RViz 已经正确加载出我们建的地图了，但是此时启动终端中会报 TF 相关的错误，这是因为我们还没有设定机器人初始位置。在 RViz 的工具栏可以看到如下图所示的几个操作按钮。

![image.png](3.%E7%BC%96%E5%86%99%20Launch%20%E5%B9%B6%E5%90%AF%E5%8A%A8%E5%AF%BC%E8%88%AA/imgs/1685117569943-4be0cb09-359e-4fec-b250-76b5ebfeb4b2.png)

2D Pose Estimate 就是用于初始化位置的工具，而 Nav2 Goal 则是设置导航目标点的工具。选中 2D Pose Estimate，然后使用鼠标左键点击地图中机器人目前所在的大概位置，不要松开左键，拖动鼠标调整机器人朝向，如果觉得设置的不够准确，可以多次设置。

设置完成后，此时终端就不再报错了，初始化位置后的地图发生了些变化，初始化完成位置之后的地图如下图所示。

![image.png](3.%E7%BC%96%E5%86%99%20Launch%20%E5%B9%B6%E5%90%AF%E5%8A%A8%E5%AF%BC%E8%88%AA/imgs/1685118024569-1deeae55-a19f-4544-ad5b-fc183c86cfcf.png)



可以看到，原有的障碍物边界好像变都变大了，这个其实就是代价地图的膨胀图层，膨胀图层是 Navigation 2 为了防止机器人和障碍物发生碰撞，在原有的地图基础上，将图中障碍物的周围按照一定的半径进行膨胀形成的。

因为全局路径规划和局部路径规划使用的地图并不同，所以在机器人周围障碍物在会在局部代价地图上进行膨胀。在 RViz 左侧显示部分，修改 Global Planner 配置，取消全局代价地图 Global Costmap 的显示，配置如下左图所示，接着就可以看到如下右图所示的局部代价地图及其膨胀层了。

![image.png](3.%E7%BC%96%E5%86%99%20Launch%20%E5%B9%B6%E5%90%AF%E5%8A%A8%E5%AF%BC%E8%88%AA/imgs/1685119751255-5fe71447-b662-476a-b470-b657461d5b05.png)

​               

![image.png](3.%E7%BC%96%E5%86%99%20Launch%20%E5%B9%B6%E5%90%AF%E5%8A%A8%E5%AF%BC%E8%88%AA/imgs/1685119587237-b6666316-b745-4818-b3f2-8c0bf63015b1.png)