Navigation2-main 项目结构详解
导航2（Navigation2）是ROS 2（Robot Operating System 2）生态系统中的核心组件，负责提供机器人导航功能。本文将详细介绍navigation2-main库的代码和文件夹结构，并说明各个文件夹的功能。
目录结构概述
navigation2-main/
├── Dockerfile
├── CMakeLists.txt
├── package.xml
├── include/
├── src/
│ ├── controllers/
│ ├── planners/
│ ├── behavior_tree/
│ ├── costmap_2d/
│ └── recoveries/
├── launch/
├── config/
├── map/
├── test/
└── doc/
详细说明

1. Dockerfile
   Dockerfile 用于构建导航2的容器化环境。该文件采用多阶段构建方式，以优化缓存和构建效率。
   ● cacher 阶段：用于缓存依赖项，提高后续构建速度。
   ● builder 阶段：负责实际编译和构建导航2的二进制文件。
   ● tester 阶段：在构建完成后执行测试，以确保构建的正确性。
   ● dever 阶段：用于开发环境，安装开发所需的工具和依赖。
   ● caddyer 和 visualizer 阶段：构建和配置用于可视化和Web服务的工具，如Caddy和gzweb。
2. CMakeLists.txt 和 package.xml
   ● CMakeLists.txt：定义了项目的构建配置，包括依赖关系、编译选项和安装规则。
   ● package.xml：描述ROS2包的元数据，如包名、版本、依赖项和维护者信息。
3. include/
   此文件夹包含了导航2库的头文件。结构通常与src/目录对应，提供各模块的公共接口。
   include/
   └── navigation2/
   ├── controllers/
   ├── planners/
   ├── behavior_tree/
   ├── costmap_2d/
   └── recoveries/
4. src/
   存放导航2的源代码，实现各项功能模块。
   ● controllers/：包含速度控制、路径跟踪等控制算法的实现。
   ● planners/：实现全局规划器和局部规划器，用于路径规划。
   ● behavior_tree/：实现行为树，用于决策和任务执行。
   ● costmap_2d/：实现成本地图构建和维护的算法。
   ● recoveries/：实现各种恢复行为，如碰撞恢复和路径重规划。
5. launch/
   存放ROS2的启动文件，定义了各个节点的启动配置和参数加载。
   launch/
   ├── navigation_launch.py
   └── bringup_launch.py
   ● navigation_launch.py：启动导航堆栈所需的各个节点。
   ● bringup_launch.py：启动整个导航系统，包括依赖的基础组件。
6. config/
   存放导航2所需的各种配置文件，通常为YAML格式。
   config/
   ├── params.yaml
   ├── planner_params.yaml
   └── costmap_params.yaml
   ● params.yaml：全局参数配置，涵盖各个节点的运行参数。
   ● planner_params.yaml：规划器特定参数配置。
   ● costmap_params.yaml：成本地图相关参数配置。
7. map/
   存放机器人导航使用的地图文件，通常为栅格地图格式（如PGM和YAML文件）。
   map/
   ├── office_map.pgm
   └── office_map.yaml
8. test/
   包含导航2各模块的测试代码，确保功能的正确性和稳定性。
   test/
   ├── controllers_test.cpp
   ├── planners_test.cpp
   └── behavior_tree_test.cpp
   ● controllers_test.cpp：测试控制器模块。
   ● planners_test.cpp：测试规划器模块。
   ● behavior_tree_test.cpp：测试行为树模块。
9. doc/
   存放项目的文档资料，包括API文档、设计文档等。通常使用Doxygen生成文档。
   doc/
   ├── Doxyfile
   └── html/
   ● Doxyfile：Doxygen配置文件，定义文档生成的参数和选项。
   ● html/：生成的HTML格式文档。
   工作区配置
   在Dockerfile中，项目采用多工作区（workspaces）结构：
   ● underlay_ws (/opt/underlay_ws)：用于存放导航2的基础依赖和源代码。
   ● overlay_ws (/opt/overlay_ws)：用于存放用户自定义的代码和配置，覆盖或扩展underlay_ws中的内容。
   通过这种结构，项目可以高效地管理依赖关系，并支持多层次的构建和开发。
   构建流程
10. 依赖安装：在cacher阶段，通过apt-get和pip3安装所需的依赖项。
11. 源码克隆：从仓库中克隆underlay和overlay的源代码。
12. 参数配置：加载和配置工作区的参数文件。
13. 编译构建：使用colcon工具进行编译和构建。
14. 测试验证：在tester阶段运行测试，确保构建的正确性。
15. 开发环境配置：在dever阶段安装开发工具，如调试器和测试工具。
    总结
    navigation2-main项目通过模块化的目录结构，清晰地组织了源代码、配置文件、测试代码和文档资料。多工作区和多阶段构建策略提高了构建效率和灵活性，使得导航2在ROS 2生态系统中具备高效、可靠的导航能力。
    如果需要更深入了解各个模块的具体实现和功能，可以参考include/和src/目录下的详细代码。同时，Doxygen生成的文档（位于doc/html/）提供了全面的API说明和使用指南。

nav2开头的文件夹
Navigation2-main 项目中 nav2_xxx 文件夹的源代码详解
在前文中，我们已经对 navigation2-main 项目的整体架构和主要文件夹进行了介绍。本节将深入探讨以 nav2_ 开头的各个模块文件夹，特别是其中的 src 目录下的源代码文件，详细介绍其功能和实现原理。

1. nav2_waypoint_follower
   src/main.cpp
   此文件包含 WaypointFollower 节点的主函数。它负责初始化 ROS 2 环境，创建 WaypointFollower 节点实例，并启动节点的执行循环。
   #include
   #include “nav2_waypoint_follower/waypoint_follower.hpp”
   #include “rclcpp/rclcpp.hpp”

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<nav2_waypoint_follower::WaypointFollower>();
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();

return 0;
}
src/waypoint_follower.cpp
WaypointFollower 类的实现文件，负责处理机器人在一系列预定航点之间的导航。核心功能包括：
● 订阅航点信息。
● 计算和发布相应的速度指令。
● 处理导航过程中的反馈与调整。
2. nav2_mppi_controller
src/parameters_handler.cpp
此文件实现了 ParametersHandler 类，用于动态管理和更新控制器的参数。主要功能包括：
● 订阅参数更改事件。
● 根据新参数调整控制器行为。
● 提供线程安全的参数访问机制。
src/noise_generator.cpp
NoiseGenerator 类的实现文件，负责在控制指令中引入随机噪声，以模拟环境中的不确定性和扰动。主要功能包括：
● 根据控制器设置生成随机噪声。
● 管理噪声生成线程，确保实时性。
● 提供噪声应用于控制指令的接口。
src/trajectory_visualizer.cpp
TrajectoryVisualizer 类用于可视化机器人生成的轨迹和路径规划结果。主要功能包括：
● 订阅并处理轨迹数据。
● 使用 ROS 的可视化工具（如 MarkerArray）展示轨迹。
● 发布优化后的路径信息，以供调试和监控使用。
3. nav2_costmap_2d
src/costmap_math.cpp
该文件包含用于成本地图计算的数学函数。核心功能包括：
● 计算点到线的距离，用于障碍物检测和避障。
● 处理成本地图中的栅格数据，更新和维护成本值。
#include <nav2_costmap_2d/costmap_math.hpp>
#include

double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
{
double A = pX - x0;
double B = pY - y0;
double C = x1 - x0;
double D = y1 - y0;

double dot = A * C + B * D;
double len_sq = C * C + D * D;
double param = dot / len_sq;

double xx, yy;

if (param < 0) {
xx = x0;
yy = y0;
} else if (param > 1) {
xx = x1;
yy = y1;
} else {
xx = x0 + param * C;
yy = y0 + param * D;
}

return distance(pX, pY, xx, yy);
}
4. nav2_behaviors
src/main.cpp
该文件包含行为服务器节点的主函数，负责初始化和运行行为服务器，用于处理各种导航恢复行为。
#include
#include “nav2_behaviors/behavior_server.hpp”
#include “rclcpp/rclcpp.hpp”

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto recoveries_node = std::make_shared<behavior_server::BehaviorServer>();

rclcpp::spin(recoveries_node->get_node_base_interface());
rclcpp::shutdown();

return 0;
}
5. nav2_util
src/base_footprint_publisher.cpp
BaseFootprintPublisher 类用于发布机器人的基础轮廓（Base Footprint），提供给其他模块（如成本地图）使用。主要功能包括：
● 获取机器人当前的位置和姿态。
● 发布基础轮廓的位置信息，以供其他模块进行避障和路径规划。
#include
#include “nav2_util/base_footprint_publisher.hpp”
#include “rclcpp/rclcpp.hpp”

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<nav2_util::BaseFootprintPublisher>();
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();

return 0;
}
6. nav2_system_tests
该模块包含用于验证和测试导航系统的各种测试用例和脚本，确保导航功能的正确性和稳定性。
6.1 src/behaviors/wait/test_wait_behavior_node.cpp
实现了 WaitBehaviorTestFixture 测试夹具，用于测试等待行为节点的功能。通过多组参数测试等待时间和取消操作的有效性。
#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include “wait_behavior_tester.hpp”

using namespace std::chrono_literals;
using nav2_system_tests::WaitBehaviorTester;

class WaitBehaviorTestFixture : public ::testing::TestWithParam<std::tuple<float, float>>
{
public:
static void SetUpTestCase()
{
wait_behavior_tester = new WaitBehaviorTester();
if (!wait_behavior_tester->isActive()) {
wait_behavior_tester->activate();
}
}

static void TearDownTestCase()
{
delete wait_behavior_tester;
wait_behavior_tester = nullptr;
}

protected:
static WaitBehaviorTester * wait_behavior_tester;
};

WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr;

TEST_P(WaitBehaviorTestFixture, testSWaitBehavior)
{
float wait_time = std::get<0>(GetParam());
float cancel = std::get<1>(GetParam());

bool success = false;
int num_tries = 3;
for (int i = 0; i != num_tries; i++) {
if (cancel == 1.0) {
success = success || wait_behavior_tester->behaviorTestCancel(wait_time);
} else {
success = success || wait_behavior_tester->behaviorTest(wait_time);
}
if (success) {
break;
}
}

EXPECT_EQ(true, success);
}

INSTANTIATE_TEST_SUITE_P(
WaitBehaviorTests,
WaitBehaviorTestFixture,
::testing::Values(
std::make_tuple(1.0, 0.0),
std::make_tuple(2.0, 0.0),
std::make_tuple(5.0, 0.0),
std::make_tuple(10.0, 1.0)),
testNameGenerator
);

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);

// initialize ROS
rclcpp::init(argc, argv);

bool all_successful = RUN_ALL_TESTS();

// shutdown ROS
rclcpp::shutdown();

return all_successful;
}
6.2 src/waypoint_follower/CMakeLists.txt
配置用于编译和安装 waypoint_follower 测试用例的 CMake 指令。主要包括添加测试、链接必要的库和安装测试脚本。
ament_add_test(test_waypoint_follower
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND “$CMAK{E}_{C}URREN{T}_{S}OURC{E}_{D}IR/tes{t}_{c}as{e}_{p}y.launch"WORKIN{G}_{D}IRECTORY"${CMAKE_CURRENT_BINARY_DIR}”
TIMEOUT 180
ENV
TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
)
7. 其他重要模块
nav2_costmap_2d
除了前述的 costmap_math.cpp 文件外，costmap_2d 模块还包含用于构建和维护成本地图的核心功能。成本地图是机器人导航中用于避障和路径规划的重要数据结构。
nav2_system_tests
该模块还包含多个测试用例和脚本，用于模拟和验证不同的导航场景和故障情况。例如：
● dummy_planner 和 dummy_controller：用于模拟规划器和控制器的测试节点。
● gps_navigation：用于测试基于 GPS 的导航功能。
● updown：测试导航系统在不同高度和复杂环境下的表现。
● system_failure：模拟系统故障，测试导航系统的恢复能力。
nav2_behaviors
behaviors 模块不仅包含行为服务器，还集成了多种导航行为，如：
● 恢复行为：当遇到障碍或导航失败时，执行特定的恢复动作。
● 任务调度：管理和调度不同的导航任务，确保任务按顺序和优先级执行。
总结
navigation2-main 项目通过以 nav2_ 开头的多个模块文件夹，模块化地实现了机器人导航所需的各种功能。每个模块的 src 目录下的源代码文件负责具体的功能实现，如路径规划、成本地图计算、行为管理、控制指令生成等。这种模块化的设计不仅提高了代码的可维护性和可扩展性，也使得各个功能模块能够独立开发和测试，确保导航系统的稳定性和可靠性。
通过深入理解各个模块的源代码功能，可以更好地进行定制化开发、故障排查和功能扩展，进一步提升机器人导航系统的性能和适用范围。