编写新的控制器插件

纯追踪控制器演示动画

概述（Overview）

本教程演示如何创建自定义控制器插件（plugin）。

我们将基于这篇论文实现纯追踪（pure pursuit）路径跟踪算法，建议先阅读该论文。

本教程基于 Nav2 中现有的 Regulated Pure Pursuit 控制器的一个早期简化版本。与本教程匹配的源码可在此处查看。

环境要求（Requirements）

ROS 2（二进制安装或源码构建）
Nav2（包括依赖）
Gazebo
Turtlebot3

教程步骤（Tutorial Steps）

1- 创建新的控制器插件

我们将实现一个纯追踪控制器。本教程的带注释源码位于 navigation_tutorials 仓库的 nav2_pure_pursuit_controller 软件包中，开发自己的控制器插件时可作参考。

示例插件类 nav2_pure_pursuit_controller::PurePursuitController 继承自基类 nav2_core::Controller。基类提供了一套用于实现控制器插件的虚方法，这些方法由控制器服务器在运行时调用以计算速度命令。下表列出了各方法的功能描述及实现要求：

虚方法（Virtual method）	方法描述	需要重写？
configure()	在控制器服务器进入 on_configure 状态时调用。通常在此声明 ROS 参数并初始化控制器成员变量。该方法接收 4 个参数：父节点的弱指针、控制器名称、tf 缓冲区指针以及代价地图的共享指针。	是
activate()	在控制器服务器进入 on_activate 状态时调用。通常在此执行控制器进入激活状态前所需的操作。	是
deactivate()	在控制器服务器进入 on_deactivate 状态时调用。通常在此执行控制器进入非激活状态前所需的操作。	是
cleanup()	在控制器服务器进入 on_cleanup 状态时调用。通常在此清理控制器所占用的资源。	是
newPathReceived()	在全局规划更新时调用。该方法应只做最少的工作，例如提取所需的全局信息（如收到新路径时重置内部状态）。	是
computeVelocityCommands()	在控制器服务器需要新的速度命令以使机器人跟随全局路径时调用。返回一个 `geometry_msgs::msg::TwistStamped` 消息，表示机器人的速度命令。该方法接收 5 个参数：当前机器人位姿的引用、当前速度、指向 `nav2_core::GoalChecker` 的指针、由路径处理器插件输出的 transformed_global_plan，以及全局规划的最后一个位姿。	是
cancel()	在控制器服务器收到取消请求时调用。若未实现此方法，控制器收到取消请求时会立即停止；若实现了此方法，控制器可执行更优雅的停止操作，并在完成后通知控制器服务器。	否
setSpeedLimit()	在需要限制机器人最大线速度时调用。速度限制可用绝对值（m/s）表示，也可用相对于最大速度的百分比表示。注意，最大角速度通常会随最大线速度按比例限制，以保持机器人的行为参数不变。	是

本教程将使用 PurePursuitController::configure、PurePursuitController::newPathReceived 和 PurePursuitController::computeVelocityCommands 方法。

configure() 方法根据 ROS 参数设置成员变量，并执行所需的初始化。

  void PurePursuitController::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;
    auto node = node_.lock();

    costmap_ros_ = costmap_ros;
    tf_ = tf;
    plugin_name_ = name;
    logger_ = node->get_logger();
    clock_ = node->get_clock();

    declare_parameter_if_not_declared(
      node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(
        0.2));
    declare_parameter_if_not_declared(
      node, plugin_name_ + ".lookahead_dist",
      rclcpp::ParameterValue(0.4));
    declare_parameter_if_not_declared(
      node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(
        1.0));
    declare_parameter_if_not_declared(
      node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(
        0.1));

    node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
    node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_);
    node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_);
    double transform_tolerance;
    node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance);
    transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance);
  }

这里 plugin_name_ + ".desired_linear_vel" 获取的是控制器专用的 ROS 参数 desired_linear_vel。Nav2 支持加载多个插件，每个插件都映射到一个 ID/名称以便管理。要获取某个特定插件的参数，使用 <插件映射名>.<参数名> 的格式。例如，示例控制器映射到名称 FollowPath，获取其专用的 desired_linear_vel 参数时使用 FollowPath.desired_linear_vel。也就是说，FollowPath 充当了插件专用参数的命名空间。后文介绍参数文件时会有更详细的说明。

参数值存储在成员变量中，供后续使用。

期望速度在 computeVelocityCommands() 方法中计算，根据当前速度和位姿生成速度命令。第三个参数是指向 nav2_core::GoalChecker 的指针，用于检查是否到达目标，本示例中不使用。第四个参数是已转换到本地代价地图坐标系、并裁剪到代价地图边界内相关部分的全局规划。本示例会将其从代价地图全局坐标系转换到机器人基座坐标系，作为纯追踪算法的跟踪路径。第五个参数是全局规划的最后一个位姿，本示例中不使用。纯追踪算法通过计算速度命令使机器人尽可能紧密地跟随全局路径，该算法假设线速度恒定，根据全局路径曲率计算角速度。

  geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & velocity,
    nav2_core::GoalChecker * /*goal_checker*/,
    const nav_msgs::msg::Path & transformed_global_plan,
    const geometry_msgs::msg::PoseStamped & /*global_goal*/)
  {
    // Transform the plan from costmap's global frame to robot base frame
    nav_msgs::msg::Path transformed_plan;
    if (!nav2_util::transformPathInTargetFrame(
        transformed_global_plan, transformed_plan, *tf_,
        costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance()))
    {
      throw nav2_core::ControllerTFError(
      "Unable to transform plan pose into local frame");
    }

    // Find the first pose which is at a distance greater than the specified lookahead distance
    auto goal_pose = std::find_if(
      global_plan_.poses.begin(), global_plan_.poses.end(),
      [&](const auto & global_plan_pose) {
        return hypot(
          global_plan_pose.pose.position.x,
          global_plan_pose.pose.position.y) >= lookahead_dist_;
      })->pose;

    double linear_vel, angular_vel;

    // If the goal pose is in front of the robot then compute the velocity using the pure pursuit algorithm
    // else rotate with the max angular velocity until the goal pose is in front of the robot
    if (goal_pose.position.x > 0) {

      auto curvature = 2.0 * goal_pose.position.y /
        (goal_pose.position.x * goal_pose.position.x + goal_pose.position.y * goal_pose.position.y);
      linear_vel = desired_linear_vel_;
      angular_vel = desired_linear_vel_ * curvature;
    } else {
      linear_vel = 0.0;
      angular_vel = max_angular_vel_;
    }

    // Create and publish a TwistStamped message with the desired velocity
    geometry_msgs::msg::TwistStamped cmd_vel;
    cmd_vel.header.frame_id = pose.header.frame_id;
    cmd_vel.header.stamp = clock_->now();
    cmd_vel.twist.linear.x = linear_vel;
    cmd_vel.twist.angular.z = max(
      -1.0 * abs(max_angular_vel_), min(
        angular_vel, abs(
          max_angular_vel_)));

    return cmd_vel;
  }

其余方法虽然未被使用，但也必须重写，因此全部保留空实现。

2- 导出控制器插件

创建好自定义控制器后，需要导出控制器插件，使控制器服务器能够发现它。插件在运行时加载，若未正确导出，控制器服务器将无法找到它们。在 ROS 2 中，插件的导出和加载由 pluginlib 处理。

回到本教程，类 nav2_pure_pursuit_controller::PurePursuitController 将作为基类 nav2_core::Controller 被动态加载。

导出控制器需要提供以下两行代码：

 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller)

这里借助 pluginlib 导出插件类。Pluginlib 提供的宏 PLUGINLIB_EXPORT_CLASS 会完成所有导出工作。

这段代码建议放在文件末尾，但从技术上讲放在文件顶部也可以。

下一步在软件包的根目录中创建插件描述文件。例如本教程包中的 pure_pursuit_controller_plugin.xml 文件，该文件包含以下信息：

library path：插件库的名称及其位置。
class name：类的名称（可选）。如果未设置，将默认为 class type。
class type：类的类型。
base class：基类的名称。
description：插件的描述。

  <library path="nav2_pure_pursuit_controller">
    <class type="nav2_pure_pursuit_controller::PurePursuitController" base_class_type="nav2_core::Controller">
      <description>
        This is pure pursuit controller
      </description>
    </class>
  </library>

下一步在 CMakeLists.txt 中使用 CMake 函数 pluginlib_export_plugin_description_file() 导出插件。该函数将插件描述文件安装到 share 目录，并设置 ament 索引使其可被发现。

  pluginlib_export_plugin_description_file(nav2_core pure_pursuit_controller_plugin.xml)

插件描述文件也应添加到 package.xml 中。

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

编译后插件即被注册。可通过以下命令验证是否注册成功：

  $ ros2 plugin list

你应该会看到类似于下面的输出：

  nav2_pure_pursuit_controller:
        Plugin(name='nav2_pure_pursuit_controller::PurePursuitController', type='nav2_pure_pursuit_controller::PurePursuitController', base='nav2_core::Controller')

接下来，我们将使用这个插件。

3- 通过参数文件传入插件名称

要启用该插件，需按如下方式修改 nav2_params.yaml 文件：

  controller_server:
    ros__parameters:
      controller_plugins: ["FollowPath"]

      FollowPath:
        plugin: "nav2_pure_pursuit_controller::PurePursuitController" # In Iron and older versions, "/" was used instead of "::"
        debug_trajectory_details: True
        desired_linear_vel: 0.2
        lookahead_dist: 0.4
        max_angular_vel: 1.0
        transform_tolerance: 1.0

在上面的代码片段中，可以看到 nav2_pure_pursuit_controller::PurePursuitController 控制器映射到其 ID FollowPath。要传递插件专属参数，使用 <plugin_id>.<plugin_specific_parameter> 格式。

4- 运行纯追踪控制器插件

运行启用 Nav2 的 TurtleBot3 仿真。详细步骤请参阅快速入门，快捷命令如下：

  $ ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/path/to/your_params_file.yaml

然后进入 RViz，点击顶部的“2D Pose Estimate”按钮，按照快速入门中描述的方式在地图上指定位置。机器人完成定位后，点击“Nav2 Goal”，再点击目标位姿。控制器将驱使机器人沿全局路径行进。