提炼总结—ROS2机器人开发（第8章）

写在最前面的话

为什么做该博客？该博客的特点是什么？
随着DeepSeek、ChatGPT等AI技术的崛起，促使机器人技术发展到了新的高度，诞生了宇树科技、特斯拉为代表的人形机器人，四足机器人等等，越来越多的科技巨头涌入机器人赛道，行业对于相关人才的需求也随之达到了顶峰。本博客的内容是替你阅读所有关于机器人的经典书籍，采用书籍瘦身计划，帮你提炼出核心内容，采用最通俗易懂的语言来解释原理，将书读薄。大大缩短学习时间，助你快速成为机器人时代的佼佼者。

---

---

一、书籍介绍

《ROS2机器人开发 从入门到实践》是一本从零基础到实战落地的机器人开发指南，通过"基础理论+工具详解+项目实战"的三阶学习路径，带你掌握机器人操作系统ROS2核心架构。书中不仅包含通信机制、URDF建模等必备知识，更有SLAM导航、机械臂控制等12个工业级案例，配合Git代码库和仿真环境配置指南，帮助开发者快速实现从算法仿真到真机部署的完整闭环。无论是学生、工程师还是科研人员，都能通过本书构建系统的ROS2开发能力，抢占机器人技术前沿阵地。

---

二、第8章 使用自己的规划器和控制器导航（提炼总结）

• Navigation2导航框架通过ROS2强大的插件机制，可以将你编写的规划器或执行器加载到导航系统中调用

8.1节 掌握ROS2插件机制（提炼总结）

• Navigation2中要使用自己的规划器和控制器，则需要使用插件库pluginlib来编写插件

8.1.1节 pluginlib介绍与安装（提炼总结）

• pluginlib：是一个用于在ROS功能包中动态加载和卸载插件的C++库。例如C++中使用动态库，一般需要使用find_package查找库的位置，接下来使用ament_target_dependencies将可执行文件与所需库进行链接，最后编译可执行文件，链接器次啊能够找到并关联所需要的库。然而通过pluginlib则不需要进行提前查找和链接库，在程序中可以通过参数或设置动态地加载和卸载插件
• 安装pluginlib，默认已经安装

sudo apt install ros-$ROS_DISTRO-pluginlib -y

8.1.2节 定义插件抽象类（提炼总结）

• 如果需要创建一个简单的机器人运动控制器插件，一个机器人可以支持多种不同的运动控制方法，比如直线、旋转运动等，此时我们就可以创建一个基类接口，然后让插件继承基类，实现不同的控制方式

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

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

2、在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(){}
    };
}

#endif

定义了抽象类Motion_Controller，该类声明了两个成员函数和一个析构函数，这个类叫做抽象类，virtual用于声明虚函数，=0用于声明纯虚函数，抽象类很抽象，所以抽象类只能被继承，不能被实例化，声明纯虚函数start()表示开始运动，一个纯虚函数stop()表示停止运动，插件的抽象类表示一种规范，接下来的所有插件都要继承该抽象类

8.1.3节 编写并生成第一个插件（提炼总结）

• 编写一个旋转运动控制插件

1、在learn_pluginlib/src/motion_control_system/include/motion_control_system文件夹下新建spin_motion_controller.hpp，编写如下代码：

#ifndef SPIN_MOTION_CONTROLLER_HPP
#define SPIN_MOTION_CONTROLLER_HPP

#include "motion_control_system/motion_control_interface.hpp"

namespace motion_control_system
{
    class SpinMotionController: public MotionController
    {
        public:
            void start() override;
            void stop() override;
    };
}

#endif

在motion_control_system命名空间下定义类SpinMotionController，并让其继承于抽象类MotionController，接着又声明了start()和stop()两个成员方法
2、在learn_pluginlib/src/motion_control_system/src文件夹下新建spin_motion_controller.cpp，编写如下代码：

#include <iostream>
#include "motion_control_system/spin_motion_controller.hpp"
namespace motion_control_system
{
    void SpinMotionController::start()
    {
        std::cout << "SpinMotionController::start()" << std::endl;
    }
    void SpinMotionController::stop()
    {
        std::cout << "SpinMotionController::stop()" << std::endl;
    } 
}
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(motion_control_system::SpinMotionController, motion_control_system::MotionController)

上面是对start()和stop()的实现，PLUGINLIB_EXPORT_CLASS宏对插件进行导出，有两个参数，第一个是要导出的类，第二个是导出的类的抽象基类
3、编写插件的描述文件，在learn_pluginlib/src/motion_control_system下新建spin_motion_plugins.xml，编写如下内容：

<library path="spin_motion_controller"> 
  <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>

通过library path="spin_motion_controller"定义了一个库，其中属性path表示库的路径，其实就是动态库的名字，其子标签class制定了插件类的名字、类和抽象基类
4、修改CMakeLists.txt来生成动态库，如下代码所示：

include_directories(include)
add_library(spin_motion_controller SHARED src/spin_motion_controller.cpp)
ament_target_dependencies(spin_motion_controller pluginlib)
install(TARGETS spin_motion_controller 
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)
install(DIRECTORY include/ DESTINATION include/)
pluginlib_export_plugin_description_file(motion_control_system spin_motion_plugins.xml)

使用add_library添加了一个库文件，第一个参数是库的名字，第二个参数SHARED表示生成动态库，第三个参数是库对应的源代码文件，后面的添加以及安装指令则用于复制文件到install目录，pluginlib_export_plugin_description_file指令，用于导出插件描述文件，第一个参数是功能包的名字，第二个参数是插件描述文件的名字
5、在learn_pluginlib目录下调用colcon build进行构建，构建成功后，查看目录install/motion_control_system/lib，就可以看到动态库libspin_motion_controller.so，至此我们就成功生成了一个自己的插件库

8.1.4节 编写插件测试程序（提炼总结）

• pluginlib除了可以生成库，也提供调用库的相关类和函数

1、在learn_pluginlib/src/motion_control_system/src下新建test_plugin.cpp，编写如下代码：

#include "motion_control_system/motion_control_interface.hpp"
#include <pluginlib/class_loader.hpp>

int main(int argc, char **argv)
{
    if(argc != 2)
         return 0;
    std::string controller_name = argv[1]; 
    pluginlib::ClassLoader<motion_control_system::MotionController>  controller_loader("motion_control_system", "motion_control_system::MotionController");
    auto controller = controller_loader.createSharedInstance(controller_name);
    controller->start();
    controller->stop();
    return 0;
}

2、修改CMakelists.txt，添加可执行文件test_plugin并安装，代码如下：

add_executable(test_plugin src/test_plugin.cpp)
ament_target_dependencies(test_plugin pluginlib)
install(TARGETS test_plugin DESTINATION lib/${PROJECT_NAME})

3、重新构建功能包，执行source并运行test_plugin，运行指令如下所示：

colcon build

source install/setup.bash

ros2 run motion_control_system test_plugin motion_control_system/SpinMotionController

指令的最后参数motion_control_system/SpinMotionController表示控制器插件类的名字，该名字要和spin_motion_plugins.xml中class标签name属性保持一致

8.2节 自定义导航规划器（提炼总结）

8.2.1节 自定义规划器介绍（提炼总结）

• 在Navigation2中机器人位姿使用消息接口geometry_msgs/msg/PoseStamped表示，路径使用消息接口nav_msgs/msg/Path表示，导航使用的占据格栅地图，其对应消息接口是nav_msgs/msg/OccupancyGrid

8.2.2节 搭建规划器插件框架（提炼总结）

1、新建目录chapt8/chapt8_ws/src，将第7章所有功能包复制到src目录下，在src目录下创建功能包nav2_custom_planner，并为其添加nav2_core和pluginlib依赖

ros2 pkg create nav2_custom_planner --dependencies nav2_core pluginlib --license Apache-2.0

2、在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 <memory>
#include <string>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav_msgs/msg/path.hpp"

namespace nav2_custom_planner {
// 自定义导航规划器类
class CustomPlanner : public nav2_core::GlobalPlanner {
public:
  CustomPlanner() = default;
  ~CustomPlanner() = default;
  // 插件配置方法
  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;
  // 插件激活方法
  void activate() override;
  // 插件停用方法
  void deactivate() override;
  // 为给定的起始和目标位姿创建路径的方法
  nav_msgs::msg::Path
  createPlan(const geometry_msgs::msg::PoseStamped &start,
             const geometry_msgs::msg::PoseStamped &goal) override;

private:
  // 坐标变换缓存指针，可用于查询坐标关系
  std::shared_ptr<tf2_ros::Buffer> tf_;
  // 节点指针
  nav2_util::LifecycleNode::SharedPtr node_;
  // 全局代价地图
  nav2_costmap_2d::Costmap2D *costmap_;
  // 全局代价地图的坐标系
  std::string global_frame_, name_;
  // 插值分辨率
  double interpolation_resolution_;
};

} // namespace nav2_custom_planner

#endif // NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_

3、在src/nav2_custom_planner/src下新建nav2_custom_planner.cpp，编写如下代码：

#include "nav2_util/node_utils.hpp"
#include <cmath>
#include <memory>
#include <string>

#include "nav2_core/exceptions.hpp"
#include "nav2_custom_planner/nav2_custom_planner.hpp"

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)
    {
        tf_ = tf;
        node_ = parent.lock();
        name_ = name;
        costmap_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID();
        // 参数初始化
        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());
    }

    void CustomPlanner::deactivate()
    {
        RCLCPP_INFO(node_->get_logger(), "正在停用类型为 CustomPlanner 的插件 %s",
                    name_.c_str());
    }

    nav_msgs::msg::Path
    CustomPlanner::createPlan(const geometry_msgs::msg::PoseStamped &start,
                              const geometry_msgs::msg::PoseStamped &goal)
    {
        // 1.声明并初始化 global_path
        nav_msgs::msg::Path global_path;
        return global_path;
    }

} // namespace nav2_custom_planner

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner,
                       nav2_core::GlobalPlanner)

4、接下来编写插件描述文件，在src/nav2_custom_planner新建custom_planner_plugin.xml，编写如下内容：

<library path="nav2_custom_planner_plugin">
	<class name="nav2_custom_planner/CustomPlanner" type="nav2_custom_planner::CustomPlanner" base_class_type="nav2_core::GlobalPlanner">
	  <description>是一个自定义示例插件，用于生成自定义路径。</description>
	</class>
</library>

5、修改CMakelists.txt生成并导出插件

# 包含头文件目录
include_directories(include)
# 定义库名称
set(library_name ${PROJECT_NAME}_plugin)
# 创建共享库
add_library(${library_name} SHARED  src/nav2_custom_planner.cpp)
# 指定库的依赖关系
ament_target_dependencies(${library_name} nav2_core pluginlib)
# 安装库文件到指定目录
install(TARGETS ${library_name}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)
# 安装头文件到指定目录
install(DIRECTORY include/
  DESTINATION include/ )
# 导出插件描述文件
pluginlib_export_plugin_description_file(nav2_core custom_planner_plugin.xml)

6、还需要再功能包清单文件package.xml中将插件描述文件导出，修改内容如下：

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

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

8.2.3节 实现自定义规划算法（提炼总结）

1、修改src/nav2_custom_planner/src/nav2_custom_planner.cpp，新增如下内容：

   {
        // 1.声明并初始化 global_path
        nav_msgs::msg::Path global_path;
        global_path.poses.clear();
        global_path.header.stamp = node_->now();
        global_path.header.frame_id = global_frame_;

        // 2.检查目标和起始状态是否在全局坐标系中
        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.计算当前插值分辨率 interpolation_resolution_ 下的循环次数和步进值
        int total_number_of_loop =
            std::hypot(goal.pose.position.x - start.pose.position.x,
                       goal.pose.position.y - start.pose.position.y) /
            interpolation_resolution_;
        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;
            pose.header.stamp = node_->now();
            pose.header.frame_id = global_frame_;
            // 将该点放到路径中
            global_path.poses.push_back(pose);
        }

        // 5.使用 costmap 检查该条路径是否经过障碍物
        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))
            {
                unsigned char cost = costmap_->getCost(mx, my); // 获取对应栅格的代价值
                // 如果存在致命障碍物则抛出异常
                if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN(node_->get_logger(),"在(%f,%f)检测到致命障碍物，规划失败。",
                        pose.pose.position.x, pose.pose.position.y);
                    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;
    }

8.2.4节 配置导航参数并测试（提炼总结）

1、修改文件src/fishbot_navigation2/config/nav2_params.yaml，如下所示：

planner_server:
  ros__parameters:
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_custom_planner/CustomPlanner"
      interpolation_resolution: 0.1

2、重新构建功能包，启动仿真和导航，初始化位姿，设置导航目标点

ros2 launch fishbot_description gazebo_sim.launch.py 

ros2 launch fishbot_navigation2 navigation2.launch.py

导航成功

8.3节 自定义导航控制器（提炼总结）

8.3.1节 自定义控制器介绍（提炼总结）

• 前面自定义一个规划器插件类时需要继承抽象基类nav2_core::GlobalPlanner，而创建控制器时需要继承的抽象基类时nav2_core::Controller
• 控制器和规划器，都是通过生命周期进行管理的，先配置，再激活，然后使用，最后进行关闭

8.3.2节 搭建控制器插件框架（提炼总结）

1、在chapt8_ws/src目录下创建功能包nav2_custom_controller，并为其添加nav2_core和pluginlib依赖

ros2 pkg create nav2_custom_controller --dependencies nav2_core pluginlib --license Apache-2.0

2、在src/nav2_custom_controller/include/nav2_custom_controller下新建custom_controller.hpp，编写如下内容：

#ifndef NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_
#define NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_

#include <memory>
#include <string>
#include <vector>

#include "nav2_core/controller.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/robot_utils.hpp"


namespace nav2_custom_controller {

class CustomController : public nav2_core::Controller {
public:
  CustomController() = default;
  ~CustomController() override = default;
  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;
  void activate() override;
  void deactivate() 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;
  void setSpeedLimit(const double &speed_limit,
                     const bool &percentage) override;

protected:
  // 存储插件名称
  std::string plugin_name_;
  // 存储坐标变换缓存指针，可用于查询坐标关系
  std::shared_ptr<tf2_ros::Buffer> tf_;
  // 存储代价地图
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
  // 存储节点指针
  nav2_util::LifecycleNode::SharedPtr node_;
  // 存储全局代价地图
  nav2_costmap_2d::Costmap2D *costmap_;
  // 存储 setPlan 提供的全局路径
  nav_msgs::msg::Path global_plan_;
  // 参数：最大线速度角速度
  double max_angular_speed_;
  double max_linear_speed_;

  // 获取路径中距离当前点最近的点
  geometry_msgs::msg::PoseStamped
  getNearestTargetPose(const geometry_msgs::msg::PoseStamped &current_pose);
  // 计算目标点方向和当前位置的角度差
  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_

3、在src/nav2_custom_controller/src下新建custom_controller.cpp，编写如下代码：

#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 {
void CustomController::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) {
  node_ = parent.lock();
  costmap_ros_ = costmap_ros;
  tf_ = tf;
  plugin_name_ = name;

  // 声明并获取参数，设置最大线速度和最大角速度
  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_);
  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 CustomController::cleanup() {
  RCLCPP_INFO(node_->get_logger(),
              "清理控制器：%s 类型为 nav2_custom_controller::CustomController",
              plugin_name_.c_str());
}

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

void CustomController::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 *) {
    (void)pose;
    geometry_msgs::msg::TwistStamped cmd_vel;
    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) {
    return current_pose;
}

double CustomController::calculateAngleDifference(
    const geometry_msgs::msg::PoseStamped &current_pose,
    const geometry_msgs::msg::PoseStamped &target_pose) {
    (void) current_pose;
    (void) target_pose;
    return .0;
}
} // namespace nav2_custom_controller

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

4、接着编写插件描述文件，在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>

5、修改CMakeLisis.txt生成并导出插件，修改如下：

# 包含头文件目录
include_directories(include)
# 定义库名称
set(library_name ${PROJECT_NAME}_plugin)
# 创建共享库
add_library(${library_name} SHARED src/custom_controller.cpp)
# 指定库的依赖关系
ament_target_dependencies(${library_name} nav2_core pluginlib)
# 安装库文件到指定目录
install(TARGETS ${library_name}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)
# 安装头文件到指定目录
install(DIRECTORY include/
 DESTINATION include/)
# 导出插件描述文件
pluginlib_export_plugin_description_file(nav2_core nav2_custom_controller.xml)

6、在package.xml中将插件描述文件导出，修改package.xml的子标签export，内容如下：

  <export>
    <build_type>ament_cmake</build_type>
      <nav2_core plugin="${prefix}/nav2_custom_controller.xml" />
  </export>

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

8.3.3节 实现自定义控制算法（提炼总结）

1、修改custom_controller.cpp，添加如下内容：

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);
  auto angle_diff = calculateAngleDifference(pose_in_globalframe, target_pose);

  // 4.根据角度差计算线速度和角速度
  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 则原地旋转，否则直行
  if (fabs(angle_diff) > M_PI/10.0) {
    cmd_vel.twist.linear.x = .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;
  }
  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.遍历路径获取路径中距离当前点最近的索引，存储到 nearest_pose_index
  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.计算角度差，并转换到 -M_PI 到 M_PI 之间
  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;
}
} // namespace nav2_custom_controller

8.3.4节 配置导航参数并测试（提炼总结）

1、修改文件src/fishbot_navigation2/config/nav2_params.yaml下的controller_server参数

    FollowPath:
      plugin: "nav2_custom_controller::CustomController"
      max_linear_speed: 0.1
      max_angular_speed: 1.0

2、重新构建功能包，启动仿真和导航，初始化位姿，设置导航目标点

ros2 launch fishbot_description gazebo_sim.launch.py 

ros2 launch fishbot_navigation2 navigation2.launch.py

移动中观察导航终端输出，说明成功了

8.4节 小结与点评（提炼总结）

• 学习了编写ROS2插件
• 学习了插件的定义、编写、生成、和加载的过程
• 如何自定义导航的规划器和