编写新的控制器插件

• 概述

• 要求

• 教程步骤

概述

本教程介绍如何创建自己的控制器插件 plugin。

在本教程中，我们将根据这篇 论文 实现纯追踪路径跟踪算法。建议您阅读该论文。

注意：本教程基于现有的简化版本的Regulated Pure Pursuit控制器，该控制器现已包含在Nav2堆栈中。您可以在 这里 找到与本教程相匹配的源代码。

要求

• ROS 2（二进制或源码编译）

• Nav2（包括依赖项）

• Gazebo

• Turtlebot3

教程步骤

1- 创建一个新的控制器插件

我们将实现纯追踪控制器。本教程中的带注释代码可以在 navigation_tutorials 存储库的``nav2_pure_pursuit_controller``中找到。该软件包可被视为编写自己的控制器插件的参考。

我们的示例插件类 nav2_pure_pursuit_controller::PurePursuitController 继承自基类 nav2_core::Controller。基类提供了一组虚拟方法来实现控制器插件。这些方法在运行时由控制器服务器调用以计算速度命令。方法列表、描述和必要性如下表所示：

虚拟方法	方法描述	需要重写？
configure()	当控制器服务器进入on_configure状态时调用此方法。理想情况下，此方法应执行ROS参数的声明和控制器成员变量的初始化。此方法接收4个输入参数：父节点的弱指针、控制器名称、tf缓冲指针和costmap的共享指针。	是
activate()	当控制器服务器进入“on_activate”状态时调用该方法。理想情况下，该方法应该实现在控制器进入活动状态之前必要的操作。	是
deactivate()	当控制器服务器进入on_deactivate状态时调用此方法。理想情况下，此方法应实现在控制器进入非活动状态之前必要的操作。	是
cleanup()	当控制器服务器进入“on_cleanup”状态时调用该方法。理想情况下，该方法应清理为控制器创建的资源。	是
setPlan()	当全局路径更新时调用此方法。理想情况下，此方法应执行转换全局路径并存储的操作。	是
computeVelocityCommands()	当控制器服务器要求提供新的速度指令以便机器人跟随全局路径时，调用此方法。该方法返回一个`geometry_msgs::msg::TwistStamped`，表示机器人的速度指令。该方法接受两个参数：当前机器人姿态的引用和当前速度。	是
setSpeedLimit()	当需要限制机器人的最大线速度时调用此方法。速度限制可以用绝对值（m/s）或相对于最大机器人速度的百分比表示。请注意，通常情况下，最大旋转速度与最大线速度的变化成比例限制，以保持当前机器人行为不变。	是

在本教程中，我们将使用``PurePursuitController::configure``、``PurePursuitController::setPlan``和``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/名称。现在，如果我们想要检索该特定插件的参数，我们可以像上面的代码片段中所做的那样使用``<mapped_name_of_plugin>.<name_of_parameter>``。例如，我们的示例控制器映射到名称``FollowPath``，为了检索特定于“FollowPath”的``desired_linear_vel``参数，我们使用了``FollowPath.desired_linear_vel``。换句话说，``FollowPath``被用作插件特定参数的命名空间。在我们讨论参数文件（或参数文件）时，我们将会看到更多相关内容。

传入的参数被存储在成员变量中，以便在以后的阶段如果需要时使用。

在``setPlan()``方法中，我们接收到机器人要遵循的更新全局路径。在我们的示例中，我们将接收到的全局路径转换为机器人的坐标系，并将转换后的全局路径存储起来以供以后使用。

void PurePursuitController::setPlan(const nav_msgs::msg::Path & path)
{
  // Transform global path into the robot's frame
  global_plan_ = transformGlobalPlan(path);
}

期望速度的计算在`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*/)
{
  // 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，这是我们的基类。

1. 要导出控制器，我们需要提供两行代码。

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

请注意，它需要 pluginlib 来导出插件的类。Pluginlib 将提供宏 PLUGINLIB_EXPORT_CLASS，该宏执行所有导出的工作。

将这些行代码放在文件末尾是一个好的做法，但从技术上讲，你也可以写在文件顶部。

2. 下一步是在软件包的根目录中创建插件的描述文件。例如，在我们的教程软件包中创建一个名为 pure_pursuit_controller_plugin.xml 的文件。此文件包含以下信息：

• library path：插件的库名称及其位置。

• class name：类的名称。

• 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>

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

pluginlib_export_plugin_description_file(nav2_core pure_pursuit_controller_plugin.xml)

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

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

5. 编译后，应该进行注册。接下来，我们将使用此插件。

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

要启用插件，我们需要修改 nav2_params.yaml 文件如下所示：

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]

    FollowPath:
      plugin: "nav2_pure_pursuit_controller::PurePursuitController"
      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- 运行 Pure Pursuit Controller 插件。

使用启用的 Nav2 运行 Turtlebot3 模拟。有关如何运行的详细说明，请参阅 开始使用。下面是一个快捷命令：

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

然后进入RViz，并点击顶部的"2D Pose Estimate"按钮，在地图上指定位置，就像在 开始使用 中描述的那样。机器人将在地图上进行定位，然后点击"Nav2 goal"，并在您希望机器人导航到的姿态上点击。之后，控制器将使机器人沿着全局路径移动。