使用自定义路径规划算法和控制算法实现机器人导航（算法验证）

pluginlib 介绍与安装

在机器人导航中,规划器 负责根据地图生成一条可行的路径,而控制器则根据路径控制机器人运动到目标位置。很多同学在研究机器人路径规划算法和控制算法时,往往需要结合机器人来进行算法的验证,Navigation 2 导航框架通过ROS 2强大的插件机制,可以将你编写的路径规划算法和控制算法加载到导航系统中进行调用。

想要在Navigation 2中要使用自己的规划器和控制器,我们需要使用插件库pluginlib来编写插件 。pluginlib是一个用于在ROS功能包中动态加载和卸载插件的C++库。在C++中使用动态库时,一般需要在编译阶段进行显式链接。例如,当我们使用ROS客户端库rclcpp时,第一步先使用find_package查找库的位置,接下来使用ament_target_dependencies 命令将可执行文件与所需库进行链接,最后在编译可执行文件时,链接器才能够找到并关联所需的库。

通过pluginlib则不需要进行提前查找和链接库,在程序中可以通过参数或设置动态地加载和卸载插件。

其实在安装Navigatioin 2时,pluginlib已经作为依赖被安装在系统中了，所以无需重复安装。

假设我们要创建一个简单的机器人运动控制器插件,一个机器人可以支持多种不同的运动控制方法,比如直线运动、旋转运动和Z字形运动等。

此时我们就可以创建一个基类接口（最基本的控制器）,然后让插件继承基类接口,在他的基础上实现不同的控制方式。（举个例子，你想要制作抹茶奶茶，可可奶茶，他的基础都是要做奶茶，这个就是基类，然后继承这个基类的基础上，机上抹茶或者可可（插件），就实现了不同的奶茶）

简单例子引入：

创建工作空间：

在主目录下创建工作空间chapt8/learn_pluginlib/src,然后在工作空间下创建功能包motion_control_system

ros2 pkg create motion_control_system --dependencies pluginlib --license Apache-2.0

创建基类接口：

接着在learn_pluginlib/src/motion_control_system/include/motion_control_system 目录下新建头文件motion_control_interface.hpp,然后编写如代码

#ifndef MOTION_CONTROL_INTERFACE_HPP
#define MOTION_CONTROL_INTERFACE_HPP

namespace motion_control_system {

class MotionController {
public:
    virtual void start() = 0;
    virtual void stop() = 0;
    virtual ~MotionController() {}
};

} // namespace motion_control_system

#endif // MOTION_CONTROL_INTERFACE_HPP

通俗解释：这个头文件是"控制器插件的身份证"

这个头文件 motion_control_interface.hpp 是一个抽象基类头文件

在ROS 2插件系统中，这个头文件是插件系统的基础：

1. 它是"抽象包"（提供接口的包）
2. 所有插件包（具体控制器实现）都需要依赖它
3. 应用包（导航系统）也需要依赖它来使用插件

没有这个头文件，插件系统就无法工作，因为系统不知道"控制器"应该有哪些方法。

头文件相规定了：

• 所有控制器必须有的基本功能（start和stop）
• 一个统一的接口，让系统知道如何与控制器交互
• 代码解耦，让系统和插件可以独立开发

在ROS 2中，这是实现"插件机制"的关键一步，就像给所有控制器贴上了一个统一的标签，让系统能轻松识别和使用它们。

为什么需要这个？

想象一下，你想要开发一个机器人控制器系统，希望它能支持多种运动方式（直线、旋转、Z字形等）。但你不希望在核心代码中硬编码所有运动方式，因为：

1. 每增加一种新运动方式，就要修改核心代码
2. 无法在运行时切换不同的运动方式
3. 代码会变得混乱，难以维护

所以，你需要一个"通用规则"，让所有运动控制器都遵循这个规则。

头文件的具体作用

#ifndef MOTION_CONTROL_INTERFACE_HPP
#define MOTION_CONTROL_INTERFACE_HPP

namespace motion_control_system {

class MotionController {
public:
    virtual void start() = 0;   // 必须实现的开始方法
    virtual void stop() = 0;    // 必须实现的停止方法
    virtual ~MotionController() {} // 虚析构函数，确保正确释放内存
};

} // namespace motion_control_system

#endif // MOTION_CONTROL_INTERFACE_HPP

1. 定义接口规范：

   • start()：所有控制器必须实现的"开始运动"方法
   • stop()：所有控制器必须实现的"停止运动"方法

2. 强制实现：

   • = 0 表示这是纯虚函数，意味着任何继承MotionController的类必须实现这两个方法

3. 提供统一接口：

   • 应用程序（如导航系统）只需要知道"这是一个MotionController"，而不需要知道具体是哪种控制器
   • 例如：MotionController* controller = new SpinMotionController();

4. 实现解耦：

   • 导航系统（应用）和具体控制器（插件）可以独立开发
   • 不需要在导航系统中硬编码所有控制器的实现

举个简单例子

// 旋转控制器实现
class SpinMotionController : public MotionController {
public:
    void start() override {
        std::cout << "开始旋转运动" << std::endl;
    }
    
    void stop() override {
        std::cout << "停止旋转运动" << std::endl;
    }
};

// 直线控制器实现
class StraightMotionController : public MotionController {
public:
    void start() override {
        std::cout << "开始直线运动" << std::endl;
    }
    
    void stop() override {
        std::cout << "停止直线运动" << std::endl;
    }
};

当导航系统需要使用控制器时，它只需要：

MotionController* controller = createController("spin"); // 创建旋转控制器
controller->start(); // 调用开始方法

创建插件

在learn_pluginlib/src/motion_control_system/include/motion_control_system 文件夹下新建spin_motion_controller.hpp,然后编写代码如下：

#include <iostream>  // 包含标准输入输出流库
#include "motion_control_system/spin_motion_controller.hpp"  // 包含自定义的旋转运动控制器头文件

namespace motion_control_system {  // 定义命名空间 motion_control_system
  void SpinMotionController::start() {  // 实现 SpinMotionController 类的 start 方法
      // 实现旋转运动控制逻辑
      std::cout << "SpinMotionController::start" << std::endl;  // 输出启动日志
  }
  void SpinMotionController::stop() {  // 实现 SpinMotionController 类的 stop 方法
      // 停止运动控制
      std::cout << "SpinMotionController::stop" << std::endl;  // 输出停止日志
  }
} // namespace motion_control_system  // 命名空间结束

#include "pluginlib/class_list_macros.hpp"  // 包含 pluginlib 宏定义头文件
PLUGINLIB_EXPORT_CLASS(motion_control_system::SpinMotionController, motion_control_system::MotionController)  // 将 SpinMotionController 注册为 MotionController 的插件类

🧩 为什么需要命名空间？

问题示例：

// 两个程序员都写了同名函数
int calculate(int a, int b) { return a + b; }  // 程序员A写的
int calculate(int a, int b) { return a * b; }  // 程序员B写的

如果两个同名函数在同一个项目里，编译器会说"我分不清你要用哪个"，导致错误。

命名空间如何解决：

// 程序员A的代码
namespace math {
    int calculate(int a, int b) { return a + b; }
}

// 程序员B的代码
namespace calculator {
    int calculate(int a, int b) { return a * b; }
}

现在，编译器知道math::calculate和calculator::calculate是两个不同的函数。

🔍 命名空间的工作原理

1. 定义命名空间 ：用namespace关键字

   namespace my_namespace {
       // 这里可以放类、函数、变量
   }

💡 实际例子（C++）

// 1. 定义命名空间
namespace math {
    int add(int a, int b) { return a + b; }
    int subtract(int a, int b) { return a - b; }
}

namespace calculator {
    int multiply(int a, int b) { return a * b; }
    int divide(int a, int b) { return a / b; }
}

// 2. 使用命名空间
int main() {
    // 使用完全限定名
    std::cout << math::add(5, 3) << std::endl;  // 输出 8
    
    // 使用using指令简化
    using namespace math;
    std::cout << add(10, 2) << std::endl;  // 输出 12
    
    // 使用另一个命名空间
    std::cout << calculator::multiply(4, 5) << std::endl;  // 输出 20
    
    return 0;
}

PLUGINLIB_EXPORT_CLASS：

PLUGINLIB_EXPORT_CLASS：宏用于将 SpinMotionController 注册为 MotionController 接口的实现类（需配合 pluginlib 库使用）。

编写插件的描述文件

在learn_pluginlib/src/motion_control_system 目录下新建spin_motion_plugins.xml

<!-- 定义插件库的路径，其实就是插件库的名字（不带扩展名，pluginlib会自动添加.so或.dll） -->
<library path="spin_motion_controller">
    <!-- 
        定义一个可加载的插件类
        - name: 插件的唯一标识（用于配置文件中引用）
        - type: 该插件类的完整C++命名空间路径（用于类型检查）
        - base_class_type: 基类的完整C++命名空间路径（用于类型检查）
    -->
    <class name="motion_control_system/SpinMotionController" 
           type="motion_control_system::SpinMotionController" 
           base_class_type="motion_control_system::MotionController">
        <!-- 该插件的简短描述（用于文档或日志） -->
        <description>Spin Motion Controller</description>
    </class>
</library>

修改CMakelists.txt来生成动态库：

include_directories(include)

# ================ 添加旋转控制器插件库 ================
# 创建一个共享库（.so文件），这是ROS 2插件必须的格式
# SHARED表示创建动态链接库
# src/spin_motion_controller.cpp是插件实现的源文件
add_library(spin_motion_controller SHARED src/spin_motion_controller.cpp)

# 确保这个库链接到pluginlib库（插件系统的核心依赖）
# 没有这个，插件无法工作
ament_target_dependencies(spin_motion_controller pluginlib)


# 安装插件库到系统
install(TARGETS spin_motion_controller
  ARCHIVE DESTINATION lib          # 静态库（通常不使用，但需要指定）
  LIBRARY DESTINATION lib          # 动态库（.so文件，插件必须）
  RUNTIME DESTINATION bin          # 可执行文件（这里不使用，但需要指定）
)

# 安装头文件到系统
install(DIRECTORY include/           # 安装include目录下的所有头文件
  DESTINATION include/             # 安装到ROS 2的include目录
)

# ================ 生成插件描述文件 ================
# 关键步骤！告诉ROS 2系统这个包包含哪些插件
# motion_control_system：包名
# spin_motion_plugins.xml：生成的插件描述文件名
pluginlib_export_plugin_description_file(motion_control_system spin_motion_plugins.xml)

CMakeLists.txt中include_directories(include)的作用

include_directories(include) 是CMake命令，它的作用是告诉编译器在编译时，进行头文件搜索路径时，也要去搜索一下include目录。

详细解释

在ROS 2中，插件系统需要在编译时能够找到该插件的头文件。这行代码让编译器知道：

• 当编译源文件时，如果遇到#include "motion_control_interface.hpp"这样的语句，编译器会去include/目录下尝试查找这个头文件

至此我们就成功地生成了一个自己的插件库,接着我们编写一个测试程序来加载并使用这个库。

编写插件测试程序

在learn_pluginlib/motion_control_system/src下新建test_plugin.cpp,然后编写代码

#include "motion_control_system/motion_control_interface.hpp"  // 包含控制器接口头文件（定义了MotionController基类）
#include <pluginlib/class_loader.hpp>  // 包含pluginlib的核心类加载器头文件

int main(int argc, char **argv) {
    // 检查命令行参数数量是否合法（需要1个参数：插件名称）
    if (argc != 2) {
        std::cerr << "Usage: " << argv[0] << " <controller_name>" << std::endl;
        return 1;  // 参数错误，返回非零值表示失败
    }
    
    // 获取命令行参数：插件名称（例如"spin"）
    std::string controller_name = argv[1];
    
    // 1. 创建插件加载器（关键步骤！）
    // 参数说明：
    //   "motion_control_system"：功能包名称（与CMakeLists.txt中的project名称一致）
    //   "motion_control_system::MotionController"：基类的完整命名空间路径
    // 这个加载器将负责从注册的插件中查找并加载指定名称的控制器
    pluginlib::ClassLoader<motion_control_system::MotionController> 
        controller_loader("motion_control_system", 
                          "motion_control_system::MotionController");
    
    // 2. 加载指定名称的插件（动态加载）
    // 返回一个共享指针（shared_ptr）指向创建的插件实例
    // 如果找不到插件，createSharedInstance会抛出异常
    auto controller = controller_loader.createSharedInstance(controller_name);
    
    // 3. 调用插件的方法（通过基类接口调用）
    // 这里使用了基类MotionController的接口，无需知道具体是哪种控制器
    controller->start();  // 启动控制器（会调用SpinMotionController的start实现）
    controller->stop();   // 停止控制器（会调用SpinMotionController的stop实现）
    
    return 0;  // 成功退出
}

修改CMakelists.txt,添加可执行文件test_plugin并安装

# ================ 添加测试可执行文件 ================
# 创建一个测试程序（用于验证插件是否正常工作）
add_executable(test_plugin src/test_plugin.cpp)

# 确保测试程序链接到pluginlib库
ament_target_dependencies(test_plugin pluginlib)

# 安装测试程序到ROS 2的安装目录
install(TARGETS test_plugin
    DESTINATION lib/${PROJECT_NAME}  # 安装到lib/motion_control_system/目录
)

重新构建功能包,接着执行source并运行test_plugin,运行指令及结果如代码

ros2 run motion_control_system test_plugin motion_control_system/SpinMotionController

ros2 run指令中最后的参数 motion_control_system/SpinMotionController 表示控制器插件类的名字,该名字要和spin_motion_plugins.xml中class标签name属性保持一致。从运行结果可以看出,我们仅通过命令行指定插件的名称就可以获取到插件库的对象并进行调用。

自定义规划器涉及的相关概念

背景： 在不同的场景下,我们希望机器人能以不同的路线移动到目标点,比如地板清扫机器人,在电量低进行回充时,需要以耗时最短的路径回到基站;在机器人清扫过程中,则可能要走"之字形"或"回字形"路径以实现对清扫区域的覆盖和遍历。Navigation 2默认的规划器的规划策略可能不符合实际的场景需求,此时就需要我们来自定义导航规划器了。

路径规划器的任务： 基于给定的机器人初始位姿、目标位姿和环境地图,计算出一条可行的路径

设涉及到的几个消息接口：

位姿消息接口：

$ ros2 interface show geometry_msgs/msg/PoseStamped
# 带参考坐标系和时间戳的姿势

#header.stamp.sec   header.stamp.nanosec      header.frame_id
std_msgs/Header header   #header的类型是std_msgs/Header
	builtin_interfaces/Time stamp   #stamp的类型是builtin_interfaces/Time
		int32 sec
		uint32 nanosec
	string frame_id     #frame_id的类型是string


#pose.position.x    pose.position.y  pose.position.z    pose.orientation.x
Pose pose
	Point position
		float64 x
		float64 y
		float64 z
	Quaternion orientation
		float64 x 0
		float64 y 0
		float64 z 0
		float64 w 1

路径消息接口：

$ ros2 interface show nav_msgs/msg/Path
#一组姿势序列，代表机器人需要遵循的路径。（一系列 PoseStamped 点的集合）

# 表示路径的帧ID。
std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id    #路径的参考坐标系
 
# 一系列姿势供参考。
geometry_msgs/PoseStamped[] poses
	std_msgs/Header header
		builtin_interfaces/Time stamp
			int32 sec
			uint32 nanosec
		string frame_id
	Pose pose
		Point position
			float64 x
			float64 y
			float64 z
		Quaternion orientation
			float64 x 0
			float64 y 0
			float64 z 0
			float64 w 1

二维网格地图消息接口：

$ ros2 interface show nav_msgs/msg/OccupancyGrid
# 此消息表示二维占据栅格地图，用于机器人导航系统中的环境表示


# ================ 消息头（包含地图的时间戳和参考坐标系） ================
std_msgs/Header header      # 作用：表示整个地图的元数据（时间戳和坐标系）
    builtin_interfaces/Time stamp       #   stamp：地图的生成时间戳（表示这个地图是什么时候创建的）
        int32 sec
        uint32 nanosec
    string frame_id    #   frame_id：地图的参考坐标系（例如"map"表示全局地图坐标系，"odom"表示里程计坐标系）




# ================ 地图元数据（描述地图的基本属性） ================
MapMetaData info    # 作用：描述地图的基本属性，与nav_msgs/MapMetaData消息完全一致
    builtin_interfaces/Time map_load_time   #   map_load_time：地图加载时间（与header中的stamp相关，但表示地图数据生成时间）
        int32 sec
        uint32 nanosec
    float32 resolution   #   resolution：地图分辨率（单位：米/栅格，例如0.05表示每个栅格代表0.05米）
    uint32 width   #   width：地图宽度（以栅格数量表示，例如40表示40个栅格宽）
    uint32 height     #   height：地图高度（以栅格数量表示，例如30表示30个栅格高）
    geometry_msgs/Pose origin  #   origin：地图原点（左下角坐标，通常为(0,0,0)）
        Point position  # 作用：地图原点（左下角坐标）在参考坐标系中的位置
            float64 x
            float64 y
            float64 z
        Quaternion orientation  # 作用：地图原点的朝向（四元数表示，通常为(0,0,0,1)表示无旋转）
            float64 x 0
            float64 y 0
            float64 z 0
            float64 w 1

#栅格数据存储数组
int8[] data


#   索引0：(0,0)  → 左上角第一个栅格
#   索引1：(1,0)  → (0,0)右边的栅格
#   索引width：(0,1) → (0,0)上方的栅格
#   索引width+1：(1,1) → (0,1)右边的栅格

# 内部数值取决于应用场景，但通常：
#       0：未占用（自由空间）
#       100：已占用（障碍物）
#       -1：未知区域（未测量到的区域）

假如现在我们想要获取地图坐标系下(x=1.0,y=0)位置是否有障碍物,也就是对应栅格的占据状态,第一步需要将(x=1.0,y=0)这个位置的坐标转换为相应的data数组索引,栅格的行索引row_index和列索引col_index可以通过如下所示操作：

关于自定义规划器涉及的相关概念我们就介绍到这,接下来就正式开始编写代码,创建规划器。

---

创建Nav2自定义规划器插件功能

完整流程预览：

1. 创建目录与功能包

• 新建工作空间和源目录：chapt8/chapt8_ws/src。

• 将第7章导航功能包复制到 src 目录。

• 在 src 目录下创建功能包 nav2_custom_planner，并添加 nav2_core和依赖。

2. 定义插件类

• 在 src/nav2_custom_planner/include/nav2_custom_planner/下创建 nav2_custom_planner.hpp,创建头文件

• 实现插件类 CustomPlanner：

  • 继承 nav2_core::GlobalPlanner。

  • 重写基类的五个纯虚函数：

    • configure：插件配置。

    • cleanup：插件清理。

    • activate：插件激活。

    • deactivate：插件停用。

    • createPlan：创建路径。

3. 实现插件逻辑

• 在 src/nav2_custom_planner/src/下创建 nav2_custom_planner.cpp。

• 实现 CustomPlanner类的方法逻辑：

  • 在 configure方法中初始化代价地图、全局坐标系等参数。

  • 在其他方法中添加简单日志，createPlan返回空路径作为占位符。

4. 定义插件描述文件

• 在 src/nav2_custom_planner/下新建 custom_planner_plugin.xml：

  • 使用 <class>标签声明插件信息。

  • 定义类名称、类型、以及基类类型。

5. 修改 CMakeLists.txt

• 添加共享库的创建与安装逻辑：

  • 创建插件库文件。

  • 指定插件依赖。

  • 安装库文件、头文件和描述文件。

  • 使用 pluginlib_export_plugin_description_file导出插件描述文件。

6. 修改 package.xml

• 在 <export>标签下添加插件描述文件导出声明：

  <nav2_core plugin="${prefix}/custom_planner_plugin.xml" />

7. 构建功能包

• 构建工作空间：colcon build --packages-select nav2_custom_planner。

• 验证生成结果：

  • 动态库位于 install/nav2_custom_planner/lib。

  • 插件描述文件位于 install/nav2_custom_planner/share/nav2_custom_planner

步骤一：创建工作空间：

新建目录chapt8/chapt8_ws/src,然后将第7章导航的所有功能包复制到src目录下,在src 目录下创建功能包nav2_custom_planner,并为其添加nav2_core和pluginlib依赖。

步骤二：构建规划器框架

在src/nav2_custom_planner/include/nav2_custom_planner 下新建nav2_custom_planner.hpp,然后编写代码

// 头文件保护：防止重复包含
// 作用：确保这个头文件只被编译一次
#ifndef NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_
#define NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_

// ================== 核心头文件 ==================
#include                 // 智能指针（如shared_ptr）的头文件
#include                 // 字符串处理
#include "geometry_msgs/msg/point.hpp"     // 3D点消息类型
#include "geometry_msgs/msg/pose_stamped.hpp" // 带时间戳的位姿消息
#include "rclcpp/rclcpp.hpp"      // ROS 2的核心C++接口
#include "nav2_core/global_planner.hpp"    // Nav2全局规划器"基类"
#include "nav2_costmap_2d/costmap_2d_ros.hpp" // 2D代价地图（表示环境障碍）
#include "nav2_util/lifecycle_node.hpp"    // 是rclcpp的子类，也可以去调用一些最基本的方法
#include "nav2_util/robot_utils.hpp"       // 机器人工具函数（如坐标转换）
#include "nav_msgs/msg/path.hpp"          // 路径消息（包含多个位姿点）

// 命名空间：避免命名冲突，将自定义代码放在nav2_custom_planner中
namespace nav2_custom_planner {

// ================== 自定义规划器类 ==================
// 作用：这是一个自定义的全局路径规划器，用于在ROS 2导航系统中
//       生成从起点到目标点的完整路径（如机器人从A点走到B点的路线）
// 为什么需要自定义？因为Nav2自带的规划器可能不满足特定需求（比如需要特殊算法、避开特定区域等）
class CustomPlanner : public nav2_core::GlobalPlanner {
public:
// 默认构造函数（空实现）
// 作用：创建规划器对象时调用
CustomPlanner() = default;

// 默认析构函数（空实现）
// 作用：销毁规划器对象时调用
~CustomPlanner() = default;

// ================== 必须重写的Nav2核心函数 ==================

// 配置函数：初始化规划器
// 参数：
//   parent：父节点（用于获取ROS 2功能，如日志、参数）
//   name：规划器的名称（在配置文件中指定）
//   tf：坐标变换缓存（用于在不同坐标系间转换位置，如map→base_link）
//   costmap_ros：代价地图（表示环境障碍，用于规划路径）
// 作用：在规划器启动前进行初始化设置
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

// 清理函数：销毁规划器
// 作用：在规划器停止前释放资源
void cleanup() override;

// 激活函数：启动规划器
// 作用：当节点进入"active"状态时调用，开始生成路径
void activate() override;

// 停用函数：停止规划器
// 作用：当节点进入"inactive"状态时调用，暂停路径生成
void deactivate() override;

// 路径规划算法实现
// 参数：
//   start：起点（包含位置和时间戳）
//   goal：目标点（包含位置和时间戳）
// 返回：生成的路径Path（包含多个位姿点的数组）
// 作用：这是规划器最重要的功能！根据起点和目标点
//       生成一条避开障碍物的路径（比如用A*算法）
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped &start,
const geometry_msgs::msg::PoseStamped &goal
) override;

private:
// ================== 私有成员变量 ==================

// 坐标变换缓存：用于在不同坐标系间转换位置
// 例如：将"map"坐标系中的点转换为"base_link"坐标系中的点
std::shared_ptr<tf2_ros::Buffer> tf_;

// 节点指针：指向当前ROS 2节点
// 作用：用于获取参数、发布日志等
nav2_util::LifecycleNode::SharedPtr node_;

// 作用：地图中每个栅格表示"可走/障碍/未知"，用于路径规划
nav2_costmap_2d::Costmap2D *costmap_;

// 全局坐标系名称：如"map"（地图坐标系）
// 作用：确保所有坐标转换使用正确坐标系
std::string global_frame_, name_;

// 插值分辨率：路径点之间的距离（单位：米）
// 例如：0.1表示路径点间隔0.1米
double interpolation_resolution_;
};

} // namespace nav2_custom_planner

#endif // NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_

代码中冒号的三种主要用法

1️⃣ 类继承（最常用！）

class CustomPlanner : public nav2_core::GlobalPlanner {

• 作用 ：表示CustomPlanner类继承 自nav2_core::GlobalPlanner类
  • public表示"公开继承"（儿子能直接用父亲的公共方法）
• 单冒号表示CustomPlanner类的父类是GlobalPlanner
• 双冒号表示GlobalPlanner 是在nav2_core命名空间中的（防止同名出错）

2️⃣ 范围解析运算符（::）

PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner,
                       nav2_core::GlobalPlanner)

• 作用：指定"属于哪个命名空间"，就像"在哪个文件夹里找文件"
• nav2_custom_planner::CustomPlanner：表示"在nav2_custom_planner命名空间中的CustomPlanner类"
• nav2_core::GlobalPlanner：表示"在nav2_core命名空间中的GlobalPlanner类"

3️⃣ 成员初始化列表（在构造函数中）

// 虽然在提供的代码里没有看到，但这是C++中非常重要的用法
CustomPlanner(int x) : a(x), b(y) {
    // 构造函数体
}

• 作用 ：在构造函数中初始化 类的成员变量
  • a(x) 表示用参数x初始化成员变量a
  • b(y) 表示用参数y初始化成员变量b

3️⃣ 类成员函数定义（必须用！）

void CustomPlanner::configure(...) {

• C++解释 ：configure是CustomPlanner类的成员函数
  override 是 C++11 引入的关键字，用于告诉编译器："这个函数是重写基类的虚函数" 。

它就像给编译器发了个"确认信"，确保你真的在重写，而不是写了个新函数。

步骤三：创建自定义路径规划插件

接着在src/nav2_custom_planner/src下新建nav2_custom_planner.cpp,编写代码

// 包含ROS 2节点工具函数（用于参数获取、日志记录等）
#include "nav2_util/node_utils.hpp"
// 数学运算（计算距离、三角函数等）
#include <cmath>
// 智能指针（自动管理内存，避免泄漏）
#include <memory>
// 字符串处理（参数名拼接等）
#include <string>

// Nav2核心异常处理（用于规划失败时抛出异常）
#include "nav2_core/exceptions.hpp"
// 自定义规划器头文件（定义类结构）
#include "nav2_custom_planner/nav2_custom_planner.hpp"

// 命名空间：将代码组织在nav2_custom_planner中
namespace nav2_custom_planner {

    // ================== 配置函数：初始化规划器 ==================
    void CustomPlanner::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, 
        std::string name,
        std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) 
    {
        // 1. 保存坐标变换工具（用于坐标系转换）
        // 例如：将"base_link"坐标系的点转换为"map"坐标系
        tf_ = tf;
        
        // 2. 保存节点指针（用于获取参数、发布日志等）
        // parent.lock()：将弱指针转换为强指针（确保生命周期）
        node_ = parent.lock();
        
        // 3. 保存规划器名称（用于日志和参数）
        name_ = name;
        
        // 4. 保存代价地图指针（地图数据）
        // costmap_ros->getCostmap()：获取底层Costmap2D对象
        // #Costmap2DROS 是 ROS 2 中用于管理代价地图的类
        // getCostmap() 是Costmap2DROS的一个成员函数，返回指向 Costmap2D 对象的指针
        // Costmap2D 是实际存储地图数据的类（包含栅格信息、代价值等）
        costmap_ = costmap_ros->getCostmap();
       

------------ 
        // 5. 获取全局坐标系名称（如"map"）
        // 确保所有坐标转换使用正确坐标系
        global_frame_ = costmap_ros->getGlobalFrameID();
        
        // 6. 声明并读取插值分辨率参数（路径点间隔）
        // 作用：控制路径点密度（0.1米 = 路径点间隔0.1米）
        // 如果参数未定义，则默认使用0.1米
        nav2_util::declare_parameter_if_not_declared(
            node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));
        
        // 从参数服务器读取实际值
        node_->get_parameter(name_ + ".interpolation_resolution",
                            interpolation_resolution_);
        
        // 💡 关键逻辑：这里在"设置规划器的参数"
        // 类似于手机地图设置"路线精度"：值越小路径越平滑但计算量越大
    }

    // ================== 清理函数：释放资源 ==================
    void CustomPlanner::cleanup()
    {
        // 打印日志：表示规划器正在被清理
        RCLCPP_INFO(node_->get_logger(), "正在清理类型为 CustomPlanner 的插件 %s",
                    name_.c_str());
        // 注意：这里没有实际释放内存，因为智能指针自动管理
    }

    // ================== 激活函数：启动规划器 ==================
    void CustomPlanner::activate()
    {
        // 打印日志：表示规划器已启动
        RCLCPP_INFO(node_->get_logger(), "正在激活类型为 CustomPlanner 的插件 %s",
                    name_.c_str());
        // 实际激活时，规划器可以开始接收createPlan请求
    }

    // ================== 停用函数：停止规划器 ==================
    void CustomPlanner::deactivate()
    {
        // 打印日志：表示规划器已暂停
        RCLCPP_INFO(node_->get_logger(), "正在停用类型为 CustomPlanner 的插件 %s",
                    name_.c_str());
        // 停用后，规划器不再处理createPlan请求
    }

    // ================== 核心函数：创建路径 ==================
    nav_msgs::msg::Path CustomPlanner::createPlan(
        const geometry_msgs::msg::PoseStamped &start,
        const geometry_msgs::msg::PoseStamped &goal)
    {
        // 1. 创建路径对象（用于存储规划结果）
        nav_msgs::msg::Path global_path;
        global_path.poses.clear();  // 清空路径点（避免重复添加）
        global_path.header.stamp = node_->now();  // 设置当前时间戳
        global_path.header.frame_id = global_frame_;  // 设置坐标系（如"map"）

        // 2. 检查起点和目标点是否在正确坐标系
        // ROS要求所有坐标转换必须使用统一坐标系
        if (start.header.frame_id != global_frame_)
        {
            RCLCPP_ERROR(node_->get_logger(), "规划器仅接受来自 %s 坐标系的起始位置",
                         global_frame_.c_str());
            return global_path;  // 返回空路径
        }

        if (goal.header.frame_id != global_frame_)
        {
            RCLCPP_INFO(node_->get_logger(), "规划器仅接受来自 %s 坐标系的目标位置",
                        global_frame_.c_str());
            return global_path;  // 返回空路径
        }

        // 3. 计算路径点数量（根据距离和插值分辨率）
        // 计算起点到目标点的直线距离（欧氏距离）
        double distance = std::hypot(
            goal.pose.position.x - start.pose.position.x,
            goal.pose.position.y - start.pose.position.y
        );
        
        // 计算需要多少个路径点（距离/插值分辨率）
        int total_number_of_loop = distance / interpolation_resolution_;
        
        // 计算每个点的X/Y增量（从起点到目标的每一步）
        double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop;
        double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop;

        // 4. 生成直线路径（从起点到目标点）
        for (int i = 0; i < total_number_of_loop; ++i)
        {
            geometry_msgs::msg::PoseStamped pose;  // 创建一个路径点
            
            // 计算当前点的坐标（线性插值）
            pose.pose.position.x = start.pose.position.x + x_increment * i;
            pose.pose.position.y = start.pose.position.y + y_increment * i;
            pose.pose.position.z = 0.0;  // Z坐标通常为0（地面）
            
            // 设置时间戳和坐标系（与全局路径一致）
            pose.header.stamp = node_->now();
            pose.header.frame_id = global_frame_;
            
            // 将点添加到路径
            global_path.poses.push_back(pose);
        }

        // 5. 检查路径是否经过障碍物（关键步骤！）
        for (geometry_msgs::msg::PoseStamped pose : global_path.poses)
        {
            unsigned int mx, my;  // 栅格坐标（地图上的位置）
            
            // 将世界坐标转换为地图栅格坐标
            if (costmap_->worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my))
            {
                // 获取该栅格的代价值（0=自由，100=障碍）
                unsigned char cost = costmap_->getCost(mx, my);
                
                // 如果是致命障碍物（100），抛出异常
                if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN(node_->get_logger(),"在(%f,%f)检测到致命障碍物，规划失败。",
                        pose.pose.position.x, pose.pose.position.y);
                    // 抛出异常：通知Nav2框架路径规划失败
                    throw nav2_core::PlannerException(
                        "无法创建目标规划: " + std::to_string(goal.pose.position.x) + "," +
                        std::to_string(goal.pose.position.y));
                }
            }
        }

        // 6. 添加目标点到路径末尾并返回
        geometry_msgs::msg::PoseStamped goal_pose = goal;
        goal_pose.header.stamp = node_->now();
        goal_pose.header.frame_id = global_frame_;
        global_path.poses.push_back(goal_pose);  // 最后一个点是目标点
        
        // 💡 关键逻辑：这就像"手机地图"检查路线是否穿过大楼
        // 如果没有障碍物，返回规划好的路径；如果有障碍物，就抛出异常
        return global_path;
    }

} // namespace nav2_custom_planner

// ================== 插件导出：让Nav2能识别这个规划器 ==================
// 这行代码是必须的，告诉Nav2"这是一个合法的规划器插件"
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner,
                       nav2_core::GlobalPlanner)

什么是代价地图？

代价地图是ROS 2导航系统中的关键数据结构 ，代价地图（Costmap）本质上是一个**二维栅格数组，**表示机器人周围环境的障碍物分布。"栅格数据用单个格网单元代表点，用一系列相邻格网单元代表线，邻接格网的集合代表面，某个栅格（mx，my）表示第mx行第my列的那个格子，每个栅格（cell）都有一个代价值（cost value），表示该位置对机器人的"危险程度"。

🌐 代价值的含义

在ROS 2中，代价值的定义如下：

namespace costmap_2d {
  static const unsigned char NO_INFORMATION = 255;  // 未知区域
  static const unsigned char LETHAL_OBSTACLE = 254; // 致命障碍物（不可通过）
  static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; // 膨胀障碍物（机器人体积导致的扩展区域）
  static const unsigned char FREE_SPACE = 0;        // 自由空间（可通行）
}

世界坐标 vs 栅格坐标

类型	表示方式	用途	例子
世界坐标	(x, y) 连续值	机器人在全局环境中的位置	(5.2, 3.7) 米
栅格坐标	(mx, my) 整数值	代价地图中的位置	(125, 87)

所以我们需要将连续的世界坐标转换为整数栅格坐标，才能在数组中查找对应位置的代价值。

其实规划器就是规划算法，规划器本身是一个插件，需要插入到nav2中，取代别的规划期而进行使用

步骤三：编写插件描述性文件

在src/nav2_custom_planner下新建custom_planner_plugin.xml,编写代码

步骤四：修改CMakeLists.txt生成并导出插件

步骤五：在package.xml中将插件描述文件导出,修改package.xml的子标签export

对于Navigation 2的插件,除了CMakeList.txt外,还要求在功能包清单文件package.xml中将插件描述文件导出,修改package.xml的子标签export,内容如代码

再次构建功能包,接着查看目录install/nav2_custom_planner/lib,可以看到对应动态库已经生成了,插件描述文件也已经放到目录install/nav2_custom_planner/share/nav2_customplanner下了。

步骤五：更换规划器，配置导航参数

在Navigation 2中,规划器插件是由规划器服务器planner_server进行调用的,所以更换控制器要在planner_server下进行配置。

将第7章的自动巡检机器人的所有功能包复制到chapt8/chapt8_ws/src下,接着修改文件src/fishbot_navigation2/config/nav2_params.yaml

第7章中默认使用的规划器插件是"nav2_navfn_planner/NavfnPlanner",我们修改为"nav2_custom_planner/CustomPlanner"并对参数进行设置:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    # GridBased:
      # plugin: "nav2_navfn_planner/NavfnPlanner"
      # tolerance: 0.5
      # use_astar: false
      # allow_unknown: true
    # plugins: ["GridBased"]
    # use_sim_time: True
    GridBased:
      plugin: "nav2_custom_planner/CustomPlanner"
      interpolation_resolution: 0.1

步骤六： 打开仿真与导航，看看实际的控制效果

保存好配置,重新构建功能包。接着启动仿真和导航,通过RViz初始化位置后,设置导航目标点,当设置的目标点无障碍时,可以看到导航规划出了一条直线路径。如果将目标点设置在障碍物的后面,则不会产生有效路径,观察导航终端输出则可以看到如下代码：

好了,至此我们就完成了自定义的规划器,并让机器人按照自定义的规划器规划出的路径移动,那机器人是如何根据规划出的路径移动的呢?这就是控制器要做的事情了。

---

自定义导航控制器：

要让机器人动起来,就需要我们给轮子发送速度命令

规划器已经将路径规划出来,接下来需要执行器来将路径转换成控制机器人的速度命令,这就是控制器的作用。

控制器根据规划器提供的路径来下发速度指令 ,使得机器人能够按照路径进行移动,所以路径跟踪就是控制器的主要功能。

前面自定义一个规划器插件类时需要继承抽象基类nav2_core :: GlobalPlanner,而创建控制器时需要继承的抽象基类是nav2_core :: Controller

步骤一：搭建控制器插件框架：

在chapt8_ws/src目录下创建功能包nav2_custom_controller,并为其添加nav2_core和

pluginlib依赖。

接着在src/nav2_custom_controller/include/nav2_custom_controller下新建custom_controller.

hpp,然后编写代码

// 防止重复包含（头文件保护）
// 这是C++头文件的标准写法，确保头文件只被包含一次
#ifndef NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_
#define NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_



// ====================== 必要的头文件 ======================
#include <memory>                   // 智能指针（管理内存）
#include <string>                   // 字符串处理
#include <vector>                   // 动态数组

// ROS 2导航核心头文件
#include "nav2_core/controller.hpp"  // 导航控制器基类定义
#include "rclcpp/rclcpp.hpp"         // ROS 2核心库（节点、参数等）
#include "nav2_util/robot_utils.hpp" // 机器人工具函数（坐标转换等）

// ====================== 命名空间 ======================
namespace nav2_custom_controller {
// 命名空间：防止命名冲突，将自定义控制器放在自己的空间里

// ====================== 自定义控制器类定义 ======================
class CustomController : public nav2_core::Controller {
  // 继承自ROS 2导航核心的Controller类
  // 这意味着我们的控制器会自动拥有导航系统需要的接口
  
public:
  // ====================== 构造函数和析构函数 ======================
  CustomController() = default;  // 默认构造函数（不写任何初始化）
  ~CustomController() override = default;  // 默认析构函数（确保正确释放资源）
  
  // ====================== 核心接口函数（必须实现） ======================
  // 1. 配置函数：初始化控制器
  void configure(
      const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,  // 父节点（ROS2节点）
      std::string name,                                       // 插件名称（如"custom_controller"）
      std::shared_ptr<tf2_ros::Buffer> tf,                    // TF转换器（坐标系转换）
      std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
  
  // 2. 清理函数：停止控制器时调用
  void cleanup() override;
  
  // 3. 激活函数：控制器启动时调用
  void activate() override;
  
  // 4. 停用函数：控制器停止时调用
  void deactivate() override;
  
  // 5. 核心函数：计算速度指令（必须实现！）
  geometry_msgs::msg::TwistStamped computeVelocityCommands(
      const geometry_msgs::msg::PoseStamped &pose,  // 机器人当前位置
      const geometry_msgs::msg::Twist &velocity,   // 机器人当前速度（未使用）
      nav2_core::GoalChecker * goal_checker) override;  // 目标检查器（未使用）
  
  // 6. 设置全局路径函数
  void setPlan(const nav_msgs::msg::Path &path) override;
  
  // 7. 设置速度限制函数（未使用）
  void setSpeedLimit(const double &speed_limit,
                     const bool &percentage) override;

protected:
  // ====================== 保护成员变量 ======================
  // 1. 插件名称（用于日志和参数）
  std::string plugin_name_;
  
  // 2. TF转换器（用于坐标系转换）
  std::shared_ptr<tf2_ros::Buffer> tf_;
  
  // 3. 代价地图（障碍物信息）
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
  
  // 4. ROS2节点指针（用于日志、参数等）
  nav2_util::LifecycleNode::SharedPtr node_;
  
  // 5. 代价地图指针（直接访问地图数据）
  nav2_costmap_2d::Costmap2D *costmap_;
  
  // 6. 存储全局路径（导航系统传入的路径）
  nav_msgs::msg::Path global_plan_;
  
  // 7. 最大线速度（m/s）和最大角速度（rad/s）
  double max_angular_speed_;
  double max_linear_speed_;

  // ====================== 自定义辅助函数 ======================
  // 1. 获取路径中距离当前点最近的点
  geometry_msgs::msg::PoseStamped getNearestTargetPose(const geometry_msgs::msg::PoseStamped &current_pose);
  
  // 2. 计算目标点方向和当前位置的角度差
  double calculateAngleDifference(
      const geometry_msgs::msg::PoseStamped &current_pose,
      const geometry_msgs::msg::PoseStamped &target_pose
      );
};

} // namespace nav2_custom_controller

// ====================== 头文件保护结束,标准结束标志 ======================
#endif // NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_

为什么用&？

• 避免复制大对象 ：如果传递一个大型对象（比如一个大数组、一个复杂类），用&传递就不需要复制，直接操作原始数据。

2️⃣ const是啥？

const表示"常量"，意思是"这个参数不能被修改"。

void printValue(const int &value) {
    // value = 10;  // 这行会报错！因为value是const，不能修改
    std::cout << value;
}

3️⃣ const &组合起来是啥？

const &表示"不能修改的引用" ，是C++中最常用、最安全的传递大对象的方式。

💡 为什么在函数参数中用const &？

✅ 优点1：避免不必要的复制（性能提升）

• 普通传递 （PoseStamped current_pose）：

  • 会复制整个PoseStamped对象
  • 如果对象很大，复制需要时间和内存

• 引用传递 （const PoseStamped &current_pose）：

  • 不复制，直接使用原始对象
  • 速度更快，内存更省

✅ 优点2：防止意外修改（安全）

• 如果函数不需要修改参数，用const保证函数不会修改传入的参数。如果不小心修改了，编译器会报错，避免了bug

✅ 优点3：兼容性更好

• const &可以接收常量对象 和非常量对象
• 普通引用（PoseStamped &）不能接收常量对象

// 这个函数可以接收非常量和常量
void func(const PoseStamped &pose);

// 这个函数只能接收非常量
void func2(PoseStamped &pose);

int main() {
    PoseStamped myPose;
    func(myPose);     // OK
    func(PoseStamped()); // OK（常量对象）
    
    func2(myPose);    // OK
    func2(PoseStamped()); // 错误！不能接收常量对象
}

📌 一句话总结

const &是C++中传递大对象的最佳方式：既避免了复制大对象的性能损失，又保证了函数不会意外修改传入的数据。

💡 小白调试小技巧

在你的calculateAngleDifference函数里，如果看到const &，就明白：

• 传入的是原始数据，不是副本
• 这个函数**高效又安全，**这是C++高手的写法

步骤二：创建自定义控制器插件

接着我们在src/nav2_custom_controller/src下新建custom_controller.cpp,编写代码清单8-26

中的代码。

#include "nav2_custom_controller/custom_controller.hpp"  // 自定义控制器的头文件
#include "nav2_core/exceptions.hpp"                      // 导航核心异常处理
#include "nav2_util/geometry_utils.hpp"                  // 几何计算工具（距离、角度等）
#include "nav2_util/node_utils.hpp"                      // 节点工具（参数获取等）
#include <algorithm>                                     // 标准算法库（用于查找最小值）
#include <chrono>                                        // 时间相关（用于计时）
#include <iostream>                                      // 输入输出流
#include <memory>                                        // 智能指针
#include <string>                                        // 字符串处理
#include <thread>                                        // 线程（虽然这里没用到）

namespace nav2_custom_controller {  // 命名空间：防止命名冲突
// ====================== 核心控制器类定义 ======================
class CustomController : public nav2_core::Controller {  // 继承自导航核心控制器
public:
  // ====================== 配置函数 ======================
  void configure(
      const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,  // 父节点（ROS2节点）
      std::string name,                                       // 插件名称（如"custom_controller"）
      std::shared_ptr<tf2_ros::Buffer> tf,                    // TF转换器（坐标系转换）
      std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {  // 代价地图（障碍物信息）
    
    // 1. 保存节点指针（方便后续使用）
    node_ = parent.lock();  // 将弱指针转换为强指针
    
    // 2. 保存代价地图指针（用于检查障碍物）
    costmap_ros_ = costmap_ros;
    
    // 3. 保存TF转换器（用于坐标系转换）
    tf_ = tf;
    
    // 4. 保存插件名称
    plugin_name_ = name;
    
    // 5. 声明并获取最大线速度参数（默认0.1 m/s）
    nav2_util::declare_parameter_if_not_declared(
        node_, plugin_name_ + ".max_linear_speed", rclcpp::ParameterValue(0.1));
    node_->get_parameter(plugin_name_ + ".max_linear_speed", max_linear_speed_);
    
    // 6. 声明并获取最大角速度参数（默认1.0 rad/s）
    nav2_util::declare_parameter_if_not_declared(
        node_, plugin_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
    node_->get_parameter(plugin_name_ + ".max_angular_speed", max_angular_speed_);
  }

  // ====================== 清理函数 ======================
  void cleanup() {
    RCLCPP_INFO(node_->get_logger(),  // 输出日志
                "清理控制器：%s 类型为 nav2_custom_controller::CustomController",
                plugin_name_.c_str());
  }

  // ====================== 激活函数 ======================
  void activate() {
    RCLCPP_INFO(node_->get_logger(),
                "激活控制器：%s 类型为 nav2_custom_controller::CustomController",
                plugin_name_.c_str());
  }

  // ====================== 停用函数 ======================
  void deactivate() {
    RCLCPP_INFO(node_->get_logger(),
                "停用控制器：%s 类型为 nav2_custom_controller::CustomController",
                plugin_name_.c_str());
  }

  // ====================== 核心：计算速度命令 ======================
  geometry_msgs::msg::TwistStamped CustomController::computeVelocityCommands(
      const geometry_msgs::msg::PoseStamped &pose,  // 机器人当前姿态
      const geometry_msgs::msg::Twist &,           // 机器人当前速度（未使用）
      nav2_core::GoalChecker *) {                 // 目标检查器（未使用）{
    
    // 1. 检查路径是否为空（必须有路径才能导航）
    if (global_plan_.poses.empty()) {
      throw nav2_core::PlannerException("收到长度为零的路径");
    }

    // 2. 将机器人当前姿态转换到全局规划坐标系（确保坐标一致）
    geometry_msgs::msg::PoseStamped pose_in_globalframe;
    if (!nav2_util::transformPoseInTargetFrame(
            pose, pose_in_globalframe, *tf_, global_plan_.header.frame_id, 0.1)) {
      throw nav2_core::PlannerException("无法将机器人姿态转换为全局计划的坐标系");
    }

    // 3. 获取最近的目标点（路径中离机器人最近的点）
    auto target_pose = getNearestTargetPose(pose_in_globalframe);
    
    // 4. 计算当前朝向与目标点的夹角差
    auto angle_diff = calculateAngleDifference(pose_in_globalframe, target_pose);

    // 5. 根据角度差计算速度（核心逻辑！）
    geometry_msgs::msg::TwistStamped cmd_vel;
    cmd_vel.header.frame_id = pose_in_globalframe.header.frame_id;  // 速度坐标系
    cmd_vel.header.stamp = node_->get_clock()->now();              // 时间戳
    
    // 角度差 > 0.3弧度（约17度）时：原地旋转
    if (fabs(angle_diff) > M_PI/10.0) {  // M_PI/10 ≈ 0.314弧度（18度）
      cmd_vel.twist.linear.x = 0.0;          // 停止前进
      cmd_vel.twist.angular.z = fabs(angle_diff) / angle_diff * max_angular_speed_;  // 旋转方向
    } 
    // 角度差小：直线前进
    else {
      cmd_vel.twist.linear.x = max_linear_speed_;  // 最大前进速度
      cmd_vel.twist.angular.z = 0.0;               // 不旋转
    }
    
    // 6. 打印控制指令（方便调试）
    RCLCPP_INFO(node_->get_logger(), "控制器：%s 发送速度(%f,%f)",
                plugin_name_.c_str(), cmd_vel.twist.linear.x,
                cmd_vel.twist.angular.z);
    
    return cmd_vel;  // 返回速度指令
  }

  // ====================== 速度限制设置（未使用） ======================
  void CustomController::setSpeedLimit(const double &speed_limit,
                                       const bool &percentage) {
    (void)percentage;  // 忽略参数（不使用）
    (void)speed_limit; // 忽略参数（不使用）
  }

  // ====================== 设置全局路径 ======================
  void CustomController::setPlan(const nav_msgs::msg::Path &path) {
    global_plan_ = path;  // 保存路径（用于后续计算）
  }

  // ====================== 获取最近目标点（核心算法） ======================
  geometry_msgs::msg::PoseStamped CustomController::getNearestTargetPose(
      const geometry_msgs::msg::PoseStamped &current_pose) {
    
    // 1. 找到路径中距离机器人最近的点索引
    using nav2_util::geometry_utils::euclidean_distance;  // 获取欧氏距离函数
    int nearest_pose_index = 0;  // 默认第一个点
    double min_dist = euclidean_distance(current_pose, global_plan_.poses.at(0));  // 距离
    
    // 遍历路径检查每个点
    for (unsigned int i = 1; i < global_plan_.poses.size(); i++) {
      double dist = euclidean_distance(current_pose, global_plan_.poses.at(i));
      if (dist < min_dist) {
        nearest_pose_index = i;  // 更新最近点索引
        min_dist = dist;
      }
    }
    
    // 2. 从路径中删除机器人已经经过的点（优化路径）
    global_plan_.poses.erase(
        std::begin(global_plan_.poses), 
        std::begin(global_plan_.poses) + nearest_pose_index);
    
    // 3. 返回最近点的下一个点（避免机器人"卡"在当前点）
    if (global_plan_.poses.size() == 1) {
      return global_plan_.poses.at(0);  // 如果只剩一个点，返回它
    }
    return global_plan_.poses.at(1);  // 返回下一个点
  }

  // ====================== 计算角度差（核心算法） ======================
  double CustomController::calculateAngleDifference(
      const geometry_msgs::msg::PoseStamped &current_pose,
      const geometry_msgs::msg::PoseStamped &target_pose) {
    
    // 1. 获取机器人当前朝向（弧度）
    float current_robot_yaw = tf2::getYaw(current_pose.pose.orientation);
    
    // 2. 计算目标点方向（从机器人到目标点的向量角度）
    float target_angle = std::atan2(
        target_pose.pose.position.y - current_pose.pose.position.y,
        target_pose.pose.position.x - current_pose.pose.position.x);
    
    // 3. 计算角度差（确保在-π到π之间）
    double angle_diff = target_angle - current_robot_yaw;
    if (angle_diff < -M_PI) {
      angle_diff += 2.0 * M_PI;  // 调整到-π到π
    } else if (angle_diff > M_PI) {
      angle_diff -= 2.0 * M_PI;
    }
    return angle_diff;
  }
};

// ====================== 插件导出（ROS 2必须） ======================
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_custom_controller::CustomController, nav2_core::Controller)

lock

node_ = parent.lock();  // 将弱指针转换为强指针

1. parent 是一个 WeakPtr：

   • 这是一个"弱引用"，它不增加节点的引用计数
   • 就像你借朋友的手机，但不"拥有"它，所以朋友可以随时收回手机

2. lock() 方法：

   • 它会检查节点是否还存在（手机是否还在）
   • 如果节点还存在，它会返回一个 shared_ptr（临时"所有权"）
   • 如果节点已经销毁，它会返回一个空指针（手机已经坏了）

nav2_util::declare_parameter_if_not_declared

// 不安全：会覆盖已有的参数值
node_->declare_parameter("custom_controller.max_linear_speed", 0.1);

// 安全：只在参数不存在时才声明
nav2_util::declare_parameter_if_not_declared(
    node_, "custom_controller.max_linear_speed", 0.1);

plugin_name_ + ".max_linear_speed"

plugin_name_：插件名称（如"custom_controller"）

拼接结果："custom_controller.max_linear_speed"

nav2_util::transformPoseInTargetFrame

nav2_util::transformPoseInTargetFrame(
    pose,                          // 1. 源姿态（机器人当前位置）
    pose_in_globalframe,           // 2. 目标姿态（转换后的机器人位置）
    *tf_,                         // 3. TF缓存（用于坐标系转换）
    global_plan_.header.frame_id,  // 4. 目标坐标系ID（如"map"）
    0.1                            // 5. 超时时间（秒）
);

🔍 一句话总结

这行代码的作用是：将机器人当前位置从当前坐标系（如base_link）转换到全局路径的坐标系（如map），确保导航系统中所有位置信息在同一个坐标系中进行计算。

auto target_pose = getNearestTargetPose(pose_in_globalframe);

• 功能 ：在全局路径（global_plan_）中，计算所有点与当前机器人位置的距离 ，返回最近的那个点
• 输入 ：pose_in_globalframe（机器人当前在全局坐标系中的位置）
• 输出 ：geometry_msgs::msg::PoseStamped 类型的目标点（包含位置和方向信息）
• auto ：C++关键字，自动推导变量类型 （这里会自动推导为 geometry_msgs::msg::PoseStamped）

为什么getNearestTargetPose(pose_in_globalframe)直接找出来的就是global_plan_路径上最近的那个点？

• 因为getNearestTargetPose是控制器类的成员函数，它已经"知道"当前的global_plan_（全局路径），所以不需要额外传入。就像你家里的智能音箱知道你家的地址，不需要每次问"我家地址是什么"。、

TwistStamped速度消息命令：

$ ros2 interface show geometry_msgs/msg/TwistStamped
# 带参考坐标系和时间戳的含线速度与角速度的速度消息指令

std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id
Twist twist
	Vector3  linear
		float64 x
		float64 y
		float64 z
	Vector3  angular
		float64 x
		float64 y
		float64 z

为什么需要using语句？

在ROS 2 Nav2中，euclidean_distance函数位于：

nav2_util
└── geometry_utils
    └── euclidean_distance()

如果不使用using，每次调用都需要写完整路径：

double dist = nav2_util::geometry_utils::euclidean_distance(current_pose, target_pose);

使用using后，代码变得简洁：

using nav2_util::geometry_utils::euclidean_distance;
double dist = euclidean_distance(current_pose, target_pose); // ✅ 简洁！

什么是euclidean_distance？

• 功能 ：计算两个点之间的欧氏距离（直线距离）
• 公式 ：√[(x₂-x₁)² + (y₂-y₁)²]

.at(0) 是什么？

• C++ std::vector 成员函数：安全访问第0个元素

• 为什么用 .at(0) 而不是 [0]？

  操作	安全性	用途	代码示例
  poses[0]	❌ 不安全（越界不检查）	快速访问	poses[0].pose.position.x
  poses.at(0)	✅ 安全（越界会抛出异常）	推荐用法	poses.at(0).pose.position.x

从路径中删除机器人已经经过的点（优化路径）

global_plan_.poses.erase (

std::begin (global_plan_.poses),

std::begin(global_plan_.poses)+ nearest_pose_index);

1. 关键组件拆解

代码片段	作用	说明
std::begin(global_plan_.poses)	获取向量起始迭代器	global_plan_.poses 是 std::vector<geometry_msgs::msg::PoseStamped>
+ nearest_pose_index	移动迭代器	nearest_pose_index 是整数索引（如 3）
结果	指向 global_plan_.poses[nearest_pose_index] 的迭代器	安全定位到指定点

std::atan2

是C++中计算两个坐标点之间角度的"魔法函数"，它能自动判断象限，让你不用写一堆条件判断就能得到正确的角度！

计算角度差（确保在-π到π之间）

double angle_diff = target_angle - current_robot_yaw;

if (angle_diff < -M_PI) {

angle_diff += 2.0 * M_PI; // 调整到-π到π

} else if (angle_diff > M_PI) {

angle_diff -= 2.0 * M_PI;

}

return angle_diff;

}

想象你站在一个360度的旋转平台上：

• current_robot_yaw = 你当前面朝的方向（比如170°）
• target_angle = 你想转到的方向（比如-170°，即190°）
• 角度差 = target_angle - current_robot_yaw = -170 - 170 = -340°

但机器人不能转340° （太绕了），它应该直接转20°（逆时针）。

这行代码的意思是：

"请把角度差调整成最短的旋转路径，让机器人走最近的路！"

#include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_custom_controller::CustomController, nav2_core::Controller)

这行代码是ROS 2导航系统中"插件注册的身份证"，它让自定义控制（CustomController）被Nav2框架识别为合法的导航控制器，就像给机器人装上"导航身份证"一样！

想象你开了一家机器人导航服务公司：

• nav2_core::Controller = "导航服务许可证"（Nav2框架要求所有控制器必须持有这个许可证）
• nav2_custom_controller::CustomController = 你的公司名字（自定义控制器）
• PLUGINLIB_EXPORT_CLASS = "向Nav2申请许可证的注册表"（告诉Nav2"我公司符合要求"）

步骤三：编写插件描述性文件

在src/nav2_custom_controller下新建 nav2_custom_controller.xml,编写代码

<class_libraries>
    <library path="nav2_custom_controller_plugin">
        <class type="nav2_custom_controller::CustomController" base_class_type="nav2_core::Controller">
            <description>
                自定义导航控制器
            </description>
        </class>
    </library>
</class_libraries>

步骤四：修改CMakeLists.txt生成并导出插件

步骤五：在package.xml中将插件描述文件导出,修改package.xml的子标签export

构建功能包,查看目录install/nav2_custom_planner/lib,可以看到对应动态库已经生成了,插件描述文件也已经放到目录install/nav2_custom_planner/share/nav2_custom_planner下了。

步骤五：更换控制器，配置导航参数

修改文件src/fishbot_navigation2/config/nav2_params.yaml下的controller_server参数,在之前的导航中,我们使用规划器插件"dwb_core :: DWBLocalPlanner"进行路径跟踪,现在我们将插件修改为"nav2_custom_controller :: CustomController"并对参数进行设置,完成后controller_server的配置如下代码

步骤六： 打开仿真与导航，看看实际的控制效果

如果想学习更多导航相关的路径规划或者运动控制知识,可以查看Navigation 2的相关资料。