nav2_common — 全栈共用工具箱解读

一句话：Nav2 全栈的"共用抽屉"，一个 CMake 宏统一编译规矩，四个 Python 小工具解决 launch 文件里的配置传递问题。

---

文件结构

nav2_common/
├── cmake/
│   └── nav2_package.cmake     ← CMake 宏，统一编译设置
└── nav2_common/launch/
    ├── replace_string.py      ← 工具1：文本占位符替换
    ├── rewritten_yaml.py      ← 工具2：YAML 结构智能改写
    ├── has_node_params.py     ← 工具3：检查 yaml 里有没有某节点配置
    └── parse_multirobot_pose.py ← 工具4：解析多机器人初始位置

---

Part 1：nav2_package.cmake — 统一编译规矩

背景：为什么需要它？

Nav2 有 30+ 个包，每个包都要在 CMakeLists.txt 里写编译配置。如果各写各的：

A包：用 C++14，没开警告
B包：用 C++17，开了部分警告
C包：Debug 模式，跑得慢
...
全栈代码风格不一致，质量参差不齐

解决方案：所有包在 CMakeLists.txt 里调一行宏：

nav2_package()   # 一句话，所有规矩都设好了

逐行解读

macro(nav2_package)

macro = CMake 的宏定义，类似 C 语言的 #define，调用时原地展开。

---

第1段：默认 Release 编译模式

if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
    message(STATUS "Setting build type to Release as none was specified.")
    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "..." FORCE)
endif()

人话：如果你没指定编译模式，自动用 Release（生产模式）。

四种编译模式对比：

Debug          调试模式：不优化，保留调试符号，跑得慢，文件大
               cmake -DCMAKE_BUILD_TYPE=Debug

Release        生产模式：全力优化(-O2)，去掉调试符号，跑得快
               cmake -DCMAKE_BUILD_TYPE=Release  ← Nav2 默认

MinSizeRel     最小体积：优化体积而非速度，嵌入式常用
               cmake -DCMAKE_BUILD_TYPE=MinSizeRel

RelWithDebInfo 带调试信息的 Release：速度+调试两全，但文件稍大
               cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo

实际影响：Release 模式下，MPPI 的 1000 条轨迹并行计算比 Debug 模式快 3-5 倍。

---

第2段：强制 C++17

if(NOT CMAKE_CXX_STANDARD)
    if ("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES)
        set(CMAKE_CXX_STANDARD 17)
    else()
        message(FATAL_ERROR "cxx_std_17 could not be found.")
    endif()
endif()

人话：必须用 C++17，不支持就直接报错退出。

为什么必须 C++17？Nav2 大量用到了：

// std::optional（C++17）— 可能没有值的返回类型
std::optional<nav_msgs::msg::Path> computePath(...);

// if constexpr（C++17）— 编译期条件判断
if constexpr (std::is_same_v<T, double>) { ... }

// 结构化绑定（C++17）
auto [x, y, theta] = getRobotPose();

// std::filesystem（C++17）— 文件路径处理
std::filesystem::path config_path = ...;

---

第3段：编译警告设置（最重要！）

if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    add_compile_options(
        -Wall              # 开启所有常见警告
        -Wextra            # 开启额外警告
        -Wpedantic         # 严格遵循 C++ 标准
        -Werror            # ★ 警告变错误，有警告就编译失败
        -Wdeprecated       # 使用了废弃的特性时警告
        -fPIC              # 生成位置无关代码（动态库必须）
        -Wshadow           # 变量遮蔽警告
        -Wnull-dereference # 空指针解引用警告
    )
    add_compile_options("$<$<COMPILE_LANGUAGE:CXX>:-Wnon-virtual-dtor>")
    # ↑ 只对 C++ 文件：非虚析构函数警告
endif()

最关键的是 -Werror：

// 普通项目：这段代码能编译通过（只是警告）
int x = 5;
int y;          // 未初始化变量，警告
if (x = 3) {}  // 赋值当条件，警告

// Nav2：上面的代码直接编译报错！
// 意味着 Nav2 代码里不允许存在任何编译警告
// 代码质量强制保证

各警告的实际意义：

-Wshadow：
  int x = 5;
  {
      int x = 10;  // ← 遮蔽了外层的 x，容易引发 bug，Nav2 不允许

-Wnull-dereference：
  int* ptr = nullptr;
  *ptr = 5;  // ← 明显的空指针，编译就报错

-Wnon-virtual-dtor：
  class Base {
      ~Base() {}  // ← 基类析构函数不是 virtual，多态时内存泄漏
  };              //   Nav2 的插件系统大量用多态，这个很重要

---

第4段：代码覆盖率支持

option(COVERAGE_ENABLED "Enable code coverage" FALSE)
if(COVERAGE_ENABLED)
    add_compile_options(--coverage)
    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage")
    set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} --coverage")
endif()

人话：可选功能，开启后能统计"测试跑了多少代码"。

# CI 里跑覆盖率测试：
cmake -DCOVERAGE_ENABLED=ON ..
make
# 测试跑完后生成报告：哪些代码被测到了，哪些没有

---

第5段：Windows 兼容

if(MSVC)
    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)   # 自动导出所有符号（省去 __declspec(dllexport)）
    add_compile_definitions(_USE_MATH_DEFINES) # 让 M_PI 等数学常量在 Windows 上可用
endif()

人话：在 Windows 上编译时的特殊处理。Linux/Mac 不走这里。

_USE_MATH_DEFINES 很实用：

// Linux：直接能用 M_PI
double area = M_PI * r * r;

// Windows 没有 _USE_MATH_DEFINES 时：
// error: 'M_PI' was not declared in this scope
// 加了 _USE_MATH_DEFINES 后 Windows 也能用 M_PI 了

---

实际使用方式

每个 Nav2 包的 CMakeLists.txt 开头都是：

cmake_minimum_required(VERSION 3.5)
project(nav2_mppi_controller)

find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)   ← 先找到 nav2_common

nav2_package()                       ← 调用宏，一行搞定所有编译设置

# 后面才是这个包自己的内容
find_package(rclcpp REQUIRED)
add_library(mppi_controller ...)

---

Part 2：四个 Python Launch 工具详解

工具1：ReplaceString — 文本占位符替换

是什么

把配置文件里的"占位符"文字替换成实际值，逐行文本替换，不理解 yaml 结构。

源码核心逻辑

def perform(self, context):
    # 1. 如果条件不满足，直接返回原文件不替换
    if self.__condition is None or self.__condition.evaluate(context):
        # 2. 创建临时文件（原文件不动）
        output_file = tempfile.NamedTemporaryFile(mode='w', delete=False)
        # 3. 逐行替换
        for line in input_file:
            for key, value in replacements.items():
                if key in line:
                    line = line.replace(key, value)
            output_file.write(line)
        return output_file.name   # 返回临时文件路径
    else:
        return yaml_filename      # 不替换，返回原文件

实际使用场景：多机器人命名空间替换

问题：两台机器人共用一份参数文件，但各自的 frame_id 要加前缀。

步骤1：在多机器人参数文件里写占位符：

# nav2_multirobot_params_1.yaml
amcl:
  ros__parameters:
    base_frame_id: "<robot_namespace>/base_footprint"
    odom_frame_id: "<robot_namespace>/odom"

controller_server:
  ros__parameters:
    robot_base_frame: "<robot_namespace>/base_link"

步骤2：在 bringup_launch.py 里用 ReplaceString：

params_file = ReplaceString(
    source_file=params_file,           # 原始文件
    replacements={
        '<robot_namespace>': ('/', namespace)
        #  ↑ 把 <robot_namespace> 替换成 /robot1（当namespace='robot1'）
    },
    condition=IfCondition(use_namespace),  # 只在多机器人模式时才替换
)

结果：

# 替换后（临时文件，robot1）
amcl:
  ros__parameters:
    base_frame_id: "/robot1/base_footprint"
    odom_frame_id: "/robot1/odom"

关键细节

原文件永远不修改！
替换结果写入 /tmp/tmpXXXXXX 这样的临时文件
返回临时文件路径给后续使用
进程结束后临时文件自动清理

---

工具2：RewrittenYaml — YAML 结构智能改写

是什么

比 ReplaceString 更聪明，解析 yaml 结构再修改，能做三件事。

三个核心功能

功能1：加命名空间前缀（root_key）

RewrittenYaml(
    source_file='nav2_params.yaml',
    root_key=namespace,   # namespace = 'robot1'
)

原始 yaml：

amcl:
  ros__parameters:
    max_particles: 2000

加了 root_key='robot1' 后变成：

robot1:
  amcl:
    ros__parameters:
      max_particles: 2000

为什么需要这个？

ROS2 多机器人时，节点运行在 /robot1/ 命名空间下
节点启动时会在 /robot1/amcl/ros__parameters 路径下查找参数
但原始 yaml 的路径是 /amcl/ros__parameters
加了 root_key 后路径变成 /robot1/amcl/ros__parameters ← 能找到了

功能2：替换参数值（param_rewrites）

RewrittenYaml(
    source_file='nav2_params.yaml',
    param_rewrites={
        'autostart': autostart,  # 用命令行传入的 autostart 值覆盖 yaml 里的
    },
)

实际效果：

# yaml 里原来写的：
bt_navigator:
  ros__parameters:
    autostart: true

# 如果命令行传了 autostart:=false，改写后变成：
bt_navigator:
  ros__parameters:
    autostart: false

支持路径式精确替换（修改深层嵌套参数）：

param_rewrites={
    'controller_server.ros__parameters.controller_frequency': '10.0'
    # 精确修改这一个参数，其他参数不动
}

功能3：类型自动转换（convert_types=True）

# 命令行传进来的都是字符串：
# ros2 launch ... autostart:=true max_particles:=500

# 不转换（convert_types=False）：
autostart = "true"   # 字符串，节点拿到会出问题
max_particles = "500"  # 字符串，不是整数

# 转换后（convert_types=True）：
autostart = True     # bool
max_particles = 500  # int

转换规则：

def convert(self, text_value):
    # 先试整数/浮点
    try:
        return float(text_value) if '.' in text_value else int(text_value)
    except ValueError:
        pass
    # 再试 bool
    if text_value.lower() == 'true':  return True
    if text_value.lower() == 'false': return False
    # 都不是，返回原字符串
    return text_value

实际使用场景

在几乎所有 launch 文件里都能看到这个模式：

# navigation_launch.py
configured_params = ParameterFile(
    RewrittenYaml(
        source_file=params_file,      # 读 nav2_params.yaml
        root_key=namespace,           # 加命名空间前缀
        param_rewrites={'autostart': autostart},  # 覆盖 autostart
        convert_types=True,           # 字符串自动转类型
    ),
    allow_substs=True,
)
# 然后每个节点都用这个 configured_params 作为参数来源
Node(
    package='nav2_planner',
    executable='planner_server',
    parameters=[configured_params],   # ← 用改写后的参数
)

---

工具3：HasNodeParams — 配置文件侦探

是什么

检查一个 yaml 文件里有没有某个节点的配置段，返回 'True' 或 'False' 字符串。

源码（极简）

def perform(self, context):
    data = yaml.safe_load(open(yaml_filename, 'r'))
    if self.__node_name in data.keys():
        return 'True'
    return 'False'

就这两行核心逻辑，检查 yaml 最外层有没有这个键名。

实际使用场景：slam_launch.py 里的条件启动

背景问题：

用户提供的 params_file 可能有两种情况：
  情况A：nav2_params.yaml 里有 slam_toolbox 的配置段
  情况B：nav2_params.yaml 里没有 slam_toolbox 的配置段

如果情况B时把 params_file 传给 slam_toolbox：
  → slam_toolbox 找不到自己的配置
  → 报错或用错误的默认值

解决方案：

# slam_launch.py
has_slam_toolbox_params = HasNodeParams(
    source_file=params_file,
    node_name='slam_toolbox'    # 检查 yaml 里有没有 'slam_toolbox' 这个键
)

# 情况A（有 slam_toolbox 配置）：把 params_file 传给它
IncludeLaunchDescription(
    slam_launch_file,
    launch_arguments={'slam_params_file': params_file}.items(),
    condition=IfCondition(has_slam_toolbox_params),   # has=True 时执行
)

# 情况B（没有 slam_toolbox 配置）：不传 params_file，让它用自己的默认配置
IncludeLaunchDescription(
    slam_launch_file,
    launch_arguments={'use_sim_time': use_sim_time}.items(),
    condition=UnlessCondition(has_slam_toolbox_params),  # has=False 时执行
)

两种 yaml 的区别：

# 情况A：nav2_params.yaml 里有 slam_toolbox 段
amcl:
  ros__parameters: ...
slam_toolbox:           ← 有这个键，HasNodeParams 返回 'True'
  ros__parameters:
    mode: mapping
    ...

# 情况B：nav2_params.yaml 里没有 slam_toolbox 段
amcl:
  ros__parameters: ...
bt_navigator:           ← 没有 slam_toolbox 键，HasNodeParams 返回 'False'
  ros__parameters: ...

---

工具4：ParseMultiRobotPose — 多机器人位置解析器

是什么

从命令行的字符串参数里解析出多台机器人各自的初始位置。

使用场景

启动多机器人仿真时，命令行指定每台机器人的初始位置：

ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py \
  robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0}; \
           robot2={x: 3.0, y: 2.0, yaw: 1.57}; \
           robot3={x: 5.0, y: 0.0, yaw: 3.14}"

解析结果

parsed = ParseMultiRobotPose('robots').value()
# 结果：
{
    'robot1': {'x': 1.0, 'y': 1.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0},
    'robot2': {'x': 3.0, 'y': 2.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 1.57},
    'robot3': {'x': 5.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 3.14},
}

缺省字段自动补 0.0（z、roll、pitch 通常不需要指定）。

解析逻辑

def __parse_argument(self, target_argument):
    # 从 sys.argv 里找 'robots:=...' 这个参数
    for arg in sys.argv[4:]:
        if arg.startswith(target_argument + ':='):
            return arg.replace(target_argument + ':=', '')

def value(self):
    # 按 ; 分割每台机器人
    parsed_args = args.split(';')
    for arg in parsed_args:
        key_val = arg.strip().split('=')  # robot1 = {x: 1.0, ...}
        key = key_val[0].strip()          # 'robot1'
        val = key_val[1].strip()          # '{x: 1.0, y: 1.0, yaw: 0.0}'
        robot_pose = yaml.safe_load(val)  # 用 yaml 解析位置字典
        # 补全缺省字段
        for field in ['x','y','z','roll','pitch','yaw']:
            if field not in robot_pose:
                robot_pose[field] = 0.0
        multirobots[key] = robot_pose

在 launch 文件里的用法

# cloned_multi_tb3_simulation_launch.py
robots = ParseMultiRobotPose('robots').value()

# 根据解析结果，为每台机器人生成一套启动配置
for robot_name, init_pose in robots.items():
    nav_instances_cmds.append(
        IncludeLaunchDescription(
            bringup_launch,
            launch_arguments={
                'namespace': robot_name,
                'x_pose': str(init_pose['x']),
                'y_pose': str(init_pose['y']),
                'yaw': str(init_pose['yaw']),
            }.items()
        )
    )

---

四个工具的使用场合对照表

工具	解决什么问题	用在哪里
ReplaceString	配置文件里有 <占位符>，运行时替换成实际值	多机器人参数文件的命名空间替换
RewrittenYaml	需要在运行时修改 yaml 结构（加前缀/改参数/转类型）	几乎所有 launch 文件，处理参数文件
HasNodeParams	不知道用户的 yaml 里有没有某节点的配置	slam_launch.py 判断是否传参给 slam_toolbox
ParseMultiRobotPose	命令行传入多台机器人的位置字符串，需要解析	多机器人仿真 launch 文件

---

整体数据流：参数文件怎么从磁盘到节点

磁盘上的 nav2_params.yaml
        │
        │ ReplaceString（可选）
        │   把 <robot_namespace> 换成 /robot1
        ↓
     替换后的临时文件
        │
        │ RewrittenYaml
        │   1. 加 root_key（命名空间前缀）
        │   2. 覆盖 autostart 等动态参数
        │   3. 字符串→正确类型
        ↓
     改写后的临时文件
        │
        │ ParameterFile(allow_substs=True)
        │   包装成 ROS2 参数文件对象
        ↓
     configured_params
        │
        ├── Node(parameters=[configured_params])  → controller_server
        ├── Node(parameters=[configured_params])  → planner_server
        ├── Node(parameters=[configured_params])  → bt_navigator
        └── ...（所有节点共用同一份改写后的参数）

---

一句话总结

nav2_common 是 Nav2 全栈的工具抽屉：nav2_package() 宏统一了 30+ 个包的编译规矩（C++17 + Release + 零警告容忍）；四个 Python 工具专门解决"配置文件在运行时怎么灵活传递"的问题——特别是多机器人场景下，让同一份参数文件能适配不同命名空间的机器人。