1. 从“能跑”到“能装能卸”:功能库把控制器拆成可替换件
把纯追踪控制器的转向逻辑拆成一个功能库,是我在第二台车上重调参数时才想明白的做法。原来那段代码直接贴在导航包源码里,导航框架升一次级,我就得对着接口一行行重对一遍,车一换,连编译都过不了。现在这套逻辑是一个独立的 ROS2 功能包,对外只承诺两件事:package.xml 里声明清楚依赖,插件描述文件里写清楚 Controller 的类型名。插件描述文件被 pluginlib 认到之后,导航框架只按类型名去加载,参数从 YAML 按插件名分层读进来,换车改速度、前视距离和变换容差就行,源码不用动。适合手上已经有控制算法、想让它被多个工程复用的人。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 功能库骨架:建包命令、依赖清单与目录约定
2.1 一条命令把包和依赖一起拉出来
先用一条命令把骨架和目标依赖同时生成,省去后面一边编译一边补 package.xml 的来回折腾:
bash复制ros2 pkg create nav2_pure_pursuit_controller \
--build-type ament_cmake \
--dependencies rclcpp geometry_msgs nav2_costmap_2d pluginlib nav_msgs nav2_util nav2_core tf2
这条命令会建出 ament_cmake 工程,并把列出的依赖同时写进 package.xml 的 <depend> 和 CMakeLists 的 find_package。依赖不是越多越好,但下面这张表里的八个,缺一个都会在编译或加载阶段给你脸色看:
| 依赖 | 在功能库里的角色 |
|---|---|
| rclcpp | 节点句柄、日志、参数接口 |
| geometry_msgs | Twist、TwistStamped、PoseStamped 消息 |
| nav2_costmap_2d | Costmap2DROS 句柄,局部避障判断要用 |
| pluginlib | 插件注册与动态加载 |
| nav_msgs | Path 全局路径消息 |
| nav2_util | declare_parameter_if_not_declared、欧氏距离工具 |
| nav2_core | Controller 基类,插件描述里的 base_class_type |
| tf2 | 全局路径到局部坐标系的变换 |
生成完把目录整理成下面这样,后面每一步都按这个结构落文件:
text复制nav2_pure_pursuit_controller
├── CMakeLists.txt
├── doc
│ └── lookahead_algorithm.png
├── include
│ └── nav2_pure_pursuit_controller
│ └── pure_pursuit_controller.hpp
├── nav2_pure_pursuit_controller.xml
├── package.xml
├── README.md
└── src
└── pure_pursuit_controller.cpp
include/ 下面必须多一层与包同名的目录,这层就是导出头文件的命名空间。写成 include/pure_pursuit_controller.hpp 的话,下游 #include "nav2_pure_pursuit_controller/xxx.hpp" 会直接找不到文件。doc/ 里放前视算法的示意图,不参与编译。插件描述文件放在包根目录,文件名与包名一致,是为了后面 CMake 里少一次改名。
2.2 package.xml:depend 与 exec_depend 要分开写
把生成的 package.xml 改成下面这样,重点看 <depend> 的分组和 <export> 那一段:
xml复制<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_pure_pursuit_controller</name>
<version>1.0.0</version>
<description>Pure pursuit controller</description>
<maintainer email="dev@example.com">某开发者</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>nav2_common</depend>
<depend>nav2_core</depend>
<depend>nav2_util</depend>
<depend>nav2_costmap_2d</depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>nav2_msgs</depend>
<depend>pluginlib</depend>
<depend>tf2</depend>
<exec_depend>nav2_bringup</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
<depend> 本身就是 build + export 的合写,一次声明能同时满足编译期找头文件和下游 find_package 时的依赖解析;拆成 build_depend 和 exec_depend 反而容易漏掉 export 那一半,下游用这个包时就会报缺依赖。nav2_common 提供 nav2_package() 宏,CMakeLists 里要 find_package 它,所以这里不能省。<exec_depend>nav2_bringup</exec_depend> 只在运行时要用,作用是起一套服务器做验证,不参与编译。<export><build_type> 这段看着像摆设,但 colcon 就是靠它确认构建类型,改成别的值整包都编不起来。
2.3 CMakeLists.txt:共享库、宏、安装、导出四件事
这份 CMakeLists 里真正要改的只有四处,其余是 ament 的常规配置:
cmake复制cmake_minimum_required(VERSION 3.5)
project(nav2_pure_pursuit_controller)
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(nav2_util REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(tf2 REQUIRED)
nav2_package()
include_directories(
include
)
set(dependencies
rclcpp
geometry_msgs
nav2_costmap_2d
pluginlib
nav_msgs
nav2_util
nav2_core
tf2
)
add_library(nav2_pure_pursuit_controller SHARED
src/pure_pursuit_controller.cpp)
# prevent pluginlib from using boost
target_compile_definitions(nav2_pure_pursuit_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
ament_target_dependencies(nav2_pure_pursuit_controller
${dependencies}
)
install(TARGETS nav2_pure_pursuit_controller
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_include_directories(include)
ament_export_libraries(nav2_pure_pursuit_controller)
ament_export_dependencies(${dependencies})
pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit_controller.xml)
ament_package()
第一处是 add_library(... SHARED ...)。pluginlib 加载插件走的是动态库装载,编成静态库会得到“符号能找到但装不进来”的怪结果。第二处是 target_compile_definitions 里那个 PLUGINLIB__DISABLE_BOOST_FUNCTIONS,定义为 PUBLIC,是因为 pluginlib 头文件里那段兼容代码会顺着依赖链传到下游,只加在自己这层编译期不生效。第三处是两条 install:共享库必须落到 lib,插件描述里的 library path 就是去这个目录找 .so;头文件目录整目录导出,下游编译才找得到 nav2_pure_pursuit_controller/pure_pursuit_controller.hpp。第四处是 pluginlib_export_plugin_description_file,第一个参数是基类所在的包 nav2_core,第二个是包根目录下的 xml 文件名,这行漏了,前面全白干。
ament_export_include_directories、ament_export_libraries、ament_export_dependencies 三行是给下游包的承诺,让别的功能库 find_package 时能直接拿到头文件和链接目标。
3. 类声明:继承 nav2_core::Controller 的六个必填项
3.1 头文件:命名空间、基类与重写清单
头文件是整个功能库对外暴露的契约,写成下面这样:
cpp复制#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
#include <string>
#include <vector>
#include <memory>
#include "nav2_core/controller.hpp"
#include "rclcpp/rclcpp.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace nav2_pure_pursuit_controller
{
class PurePursuitController : public nav2_core::Controller
{
public:
PurePursuitController() = default;
~PurePursuitController() override = default;
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
void cleanup() override;
void activate() override;
void deactivate() override;
void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * goal_checker) override;
void setPlan(const nav_msgs::msg::Path & path) override;
protected:
nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);
bool transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose,
const rclcpp::Duration & transform_tolerance
) const;
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::string plugin_name_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")};
rclcpp::Clock::SharedPtr clock_;
double desired_linear_vel_;
double lookahead_dist_;
double max_angular_vel_;
rclcpp::Duration transform_tolerance_ {0, 0};
