## 快速重定位导航

### 1、课程内容

1.  学习机器人快速重定位导航功能
2.  运行程序，rviz中会加载实时地图和机器人模型，机器人可以快速定位出在地图中的位置，能进行实时slam建图和导航功能

### 2、原理简介

#### 2.1简介

*   Navigation 2（Nav 2）是ROS 2中自带的导航框架，其目的是能够通过一种安全的方式使移动机器人从A点移动到B点。
*   其中Navigation 2默认的定位算法是AMCL(自适应蒙特卡洛定位) ,将默认的AMCL的定位方式更换为cartographer的定位方式，其他导航功能保持不变

#### 2.2 两种定位方式区别

*   **AMCL**：适用于已知地图的场景，如在已构建好地图的室内环境中，机器人进行重复任务或导航时，可使用 AMCL 来确定自身位置。
*   **Cartographer**：更适合在未知环境中，机器人需要同时建图和定位的场景，如机器人首次进入一个新的区域，需要快速构建地图并确定自身位置时，Cartographer 能发挥更好的作用。

### 3、准备工作

#### 3.1 内容说明
我们的主板直接打开终端，输入本节课程提及的指令即可。

#### 3.2 地图准备
*   本节课程的快速重定位导航需要先按照教程【激光雷达课程】-【cartographer建图】保存一张pbstream格式地图，和yaml文件，pbstream，yaml文件会保存在
`~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.pbstream
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.yaml`
目录下
![](/media/202512/2930246e0a424abb93318e23fb2816063079.png)

#### 3.3 导航功能

**注意：**
   **我们的主板直接打开终端**，输入本节课程提及的指令即可。
机器人车机终端启动底层传感器命令：
```
ros2 launch yahboomcar_nav laser_bringup_launch.py
```
![](/media/202512/09270698520942e68a4c14420d97d0b33979.png)

输入指令启动rviz可视化建图 。再启动cartographer节点，用于定位，【use_rviz:=true】改成true表示开启rviz可视化

```#ORIN主控
ros2 launch yahboomcar_nav localization_imu_odom.launch.py use_rviz:=true load_state_filename:=/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.pbstream
```
参数说明

```
#选择是否打开rviz，true为打开,false为不打开
use_rviz:=true 
#可替换目标地图文件
#.pbstream地图文件参考cartograph建图算法保存地图
load_state_filename:=/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.pbstream 
```

![](/media/202512/a46b8ee5bc47408f948997291b5537a07499.png)

此时可以看到地图加载进去了，但是小车定位并不准确，然后我们点击【2D Pose Estimate】，给小车设置初始位姿，根据小车在实际环境中的大概位置，在rviz中用鼠标点击拖动，小车能快速的定位到地图上。如下图所示，雷达扫描的区域与实际障碍物大致重合。

**注：如果仍然无法定位，可能是小车处于障碍物区域，请放置在四周近距离无障碍物的地方再次启动。也可以终端保持开启，直接重启小车，等待自动恢复数据后重新定位。也可能是建图的时候地图特征点过少，建图一定需要细致一点的建图。**

![image-20250813183136557](/media/202512/1a5c5380f1574cb6a18adb686a399b351914.png)

最后启动Navigation 2导航，

```
#ORIN主控
ros2 launch yahboomcar_nav navigation_cartodwb_launch.py maps:=/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.yaml

```
参数说明

```
#加载地图参数：（可替换目标地图）
maps:=/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.yaml
#.pgm文件也必须和.yaml在同路径下
```

**注：此处yahboomcar.yaml必须和yahboomcar.pbstream是同时完成建图的，即是同一个地图，参考cartograph建图算法保存地图**

![image-20250813183329084](/media/202512/bbd5c0add6e94358b3a398018f6c1bad8261.png)

出现膨胀区域即可进行导航。

单点导航，点击【2D Goal Pose】工具，然后在rviz中选择一个目标点，小车结合周围的情况，规划出一条路径并且沿着路径移动到目标点。

![image-20250813183804282](/media/202512/134c5d512ed54ff687ac51c837bed11e2507.png)

多点导航，需要把nav2的插件添加进来，

![image-20250813183839534](/media/202512/840d3004e08b496a98966fabd958f53b7006.png)

添加后，rviz显示如下，

![image-20250813183857460](/media/202512/790ffd0efa3b402db3a91519366d77048033.png)

然后点击【Waypoint/Nav Through Poses Mode】,使用rivz工具栏中的【Nav2 Goal】给定任意的目标点，

![image-20250813183919689](/media/202512/4864098a89224fafb76b21ba224647596731.png)

然后点击【Start Waypoint Following】开始规划路径导航。小车会根据选的点的先后顺序，到了目标点后会自动前往下一个点，无需进行操作。达到最后一个点后，小车停车等待下一个指令。

![image-20250813184023770](/media/202512/d78cc7f3a1654f0a837a0f9d12257caa1285.png)

### 4、查看节点通讯图

终端输入，`
ros2 run rqt_graph rqt_graph
`

如果一开始没有显示，选择【Nodes/Topics(all)】，然后点击左上角的刷新按钮。

![](/media/202512/0b6146d47cd44156b6a42fd757059cb69706.png)

### 5、查看TF树

终端输入，`
ros2 run rqt_tf_tree rqt_tf_tree
`

![](/media/202512/5dfda395c88f43a68c03a1ee3438f7304057.png)

### 6、原理讲解

快速的重定位导航关键点在于使用cartographer的定位方式替换原本navigation2默认的amcl定位方式，其他内容保持不变，以下是对替换定位方式的内容进行讲解。

源码位置：`~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/launch/`

launch目录下找到`cartographer_localization.launch.py`文件，内容如下：

*   本文件是启动cartographer节点，用于替换navigation2中默认的定位方式。对navigation2默认启动文件进行了修改，去除了原本启动amcl定位方式的节点，导航栈的其他内容保持不变。

```
# Copyright (c) 2018 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import LoadComposableNodes
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode, ParameterFile
from nav2_common.launch import RewrittenYaml

def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('nav2_bringup')

namespace = LaunchConfiguration('namespace')
    map_yaml_file = LaunchConfiguration('map')
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('params_file')
    use_composition = LaunchConfiguration('use_composition')
    container_name = LaunchConfiguration('container_name')
    container_name_full = (namespace, '/', container_name)
    use_respawn = LaunchConfiguration('use_respawn')
    log_level = LaunchConfiguration('log_level')

# Remove 'amcl' from lifecycle_nodes
    lifecycle_nodes = ['map_server']

# Map fully qualified names to relative ones so the node's namespace can be prepended.
    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
    # https://github.com/ros/geometry2/issues/32
    # https://github.com/ros/robot_state_publisher/pull/30
    # TODO(orduno) Substitute with `PushNodeRemapping`
    #              https://github.com/ros2/launch_ros/issues/56
    remappings = [('/tf', 'tf'),
                  ('/tf_static', 'tf_static')]

# Create our own temporary YAML files that include substitutions
    param_substitutions = {
        'use_sim_time': use_sim_time,
        'yaml_filename': map_yaml_file}

configured_params = ParameterFile(
        RewrittenYaml(
            source_file=params_file,
            root_key=namespace,
            param_rewrites=param_substitutions,
            convert_types=True),
        allow_substs=True)

stdout_linebuf_envvar = SetEnvironmentVariable(
        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')

declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace')

declare_map_yaml_cmd = DeclareLaunchArgument(
        'map',
        description='Full path to map yaml file to load')

declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true')

declare_params_file_cmd = DeclareLaunchArgument(
        'params_file',
        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
        description='Full path to the ROS2 parameters file to use for all launched nodes')

declare_autostart_cmd = DeclareLaunchArgument(
        'autostart', default_value='true',
        description='Automatically startup the nav2 stack')

declare_use_composition_cmd = DeclareLaunchArgument(
        'use_composition', default_value='False',
        description='Use composed bringup if True')

declare_container_name_cmd = DeclareLaunchArgument(
        'container_name', default_value='nav2_container',
        description='the name of conatiner that nodes will load in if use composition')

declare_use_respawn_cmd = DeclareLaunchArgument(
        'use_respawn', default_value='False',
        description='Whether to respawn if a node crashes. Applied when composition is disabled.')

declare_log_level_cmd = DeclareLaunchArgument(
        'log_level', default_value='info',
        description='log level')

load_nodes = GroupAction(
        condition=IfCondition(PythonExpression(['not ', use_composition])),
        actions=[
            Node(
                package='nav2_map_server',
                executable='map_server',
                name='map_server',
                output='screen',
                respawn=use_respawn,
                respawn_delay=2.0,
                parameters=[configured_params],
                arguments=['--ros-args', '--log-level', log_level],
                remappings=remappings),
            Node(
                package='nav2_lifecycle_manager',
                executable='lifecycle_manager',
                name='lifecycle_manager_localization',
                output='screen',
                arguments=['--ros-args', '--log-level', log_level],
                parameters=[{'use_sim_time': use_sim_time},
                            {'autostart': autostart},
                            {'node_names': lifecycle_nodes}])
        ]
    )

load_composable_nodes = LoadComposableNodes(
        condition=IfCondition(use_composition),
        target_container=container_name_full,
        composable_node_descriptions=[
            ComposableNode(
                package='nav2_map_server',
                plugin='nav2_map_server::MapServer',
                name='map_server',
                parameters=[configured_params],
                remappings=remappings),
            ComposableNode(
                package='nav2_lifecycle_manager',
                plugin='nav2_lifecycle_manager::LifecycleManager',
                name='lifecycle_manager_localization',
                parameters=[{'use_sim_time': use_sim_time,
                             'autostart': autostart,
                             'node_names': lifecycle_nodes}]),
        ],
    )

# Create the launch description and populate
    ld = LaunchDescription()

# Set environment variables
    ld.add_action(stdout_linebuf_envvar)

# Declare the launch options
    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_map_yaml_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_params_file_cmd)
    ld.add_action(declare_autostart_cmd)
    ld.add_action(declare_use_composition_cmd)
    ld.add_action(declare_container_name_cmd)
    ld.add_action(declare_use_respawn_cmd)
    ld.add_action(declare_log_level_cmd)

# Add the actions to launch all of the localiztion nodes
    ld.add_action(load_nodes)
    ld.add_action(load_composable_nodes)

return ld
```

launch目录下找到`navigation_cartodwb_launch.py`文件，内容如下：

这里启动了以下几个节点：

*   base_link_to_laser_tf：发布静态的TF变换；
*   map_yaml_path：加载地图文件，默认值
 为/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_navmaps/yahboom_map.yaml，用于指定要加载的地图文件路径；
cartographer_bringup_launch.py：启动cartographer纯定位的launch文件，文件位于，/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/launch/cartographer_bringup_launch.py
ORIN主控，路径在/home/yahboom/下
RDKX5主控，路径在/home/sunrise/下
另外还加载了一个导航参数配置文件cartoteb_nav_params.yaml。

```
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    package_path = get_package_share_directory('yahboomcar_nav')

use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    namespece = LaunchConfiguration('namespece', default='')
    map_yaml_path = LaunchConfiguration(
        'maps', default=os.path.join('/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav', 'maps', 'yahboomcar.yaml')) 
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        package_path, 'params', 'cartoteb_nav_params.yaml'))

return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                              description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument('namespece', default_value=namespece,
                              description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument('maps', default_value=map_yaml_path,
                              description='Full path to map file to load'),
        DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                              description='Full path to param file to load'),

IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [package_path, '/launch', '/cartographer_bringup_launch.py']),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'namespece': namespece,
                'params_file': nav2_param_path}.items(),
        ),
    ])

```

cartographer快速重定位导航