编写新的控制器插件

概述
要求
教程步骤

Animated gif of pure pursuit controller demo

概述

本教程介绍如何创建自己的控制器“插件<https://index.ros.org/p/pluginlib/>”。

在本教程中，我们将根据这篇`论文<https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf>`_实现纯追踪路径跟踪算法。建议您仔细阅读一下。

注意：本教程基于 Nav2 堆栈中现有的 Regulated Pure Pursuit 控制器的简化版本。您可以在`此处 <https://github.com/ros-planning/navigation2_tutorials/tree/126902457c5c646b136569886d6325f070c1073d/nav2_pure_pursuit_controller>`_ 找到与本教程匹配的源代码。

要求

ROS 2 (二进制或从源代码构建)
Nav2 (包括依赖项)
Gazebo
Turtlebot3

教程步骤

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

我们将实现纯追踪控制器。本教程中带注释的代码可以在 navigation_tutorials 存储库中找到，名为 nav2_pure_pursuit_controller。此包可作为编写您自己的控制器插件的参考。我们的示例插件类“nav2_pure_pursuit_controller::PurePursuitController”继承自基类“nav2_core::Controller”。基类提供了一组虚拟方法来实现控制器插件。控制器服务器在运行时调用这些方法来计算速度命令。下表列出了方法列表、它们的描述和必要性：

Virtual method	Method description	Requires override?
configure()	Method is called when controller server enters on_configure state. Ideally this method should perform declarations of ROS parameters and initialization of controller’s member variables. This method takes 4 input params: weak pointer to parent node, controller name, tf buffer pointer and shared pointer to costmap.	Yes
activate()	Method is called when controller server enters on_activate state. Ideally this method should implement operations which are necessary before controller goes to an active state.	Yes
deactivate()	Method is called when controller server enters on_deactivate state. Ideally this method should implement operations which are necessary before controller goes to an inactive state.	Yes
cleanup()	Method is called when controller server goes to on_cleanup state. Ideally this method should clean up resources which are created for the controller.	Yes
setPlan()	Method is called when the global plan is updated. Ideally this method should perform operations that transform the global plan and store it.	Yes
computeVelocityCommands()	Method is called when a new velocity command is demanded by the controller server in-order for the robot to follow the global path. This method returns a geometry_msgs::msg::TwistStamped which represents the velocity command for the robot to drive. This method passes 2 parameters: reference to the current robot pose and its current velocity.	Yes
cancel()	Method is called when the controller server receives a cancel request. If this method is unimplemented, the controller will immediately stop when receiving a cancel request. If this method is implemented, the controller can perform a more graceful stop and signal the controller server when it is done.	No
setSpeedLimit()	Method is called when it is required to limit the maximum linear speed of the robot. Speed limit could be expressed in absolute value (m/s) or in percentage from maximum robot speed. Note that typically, maximum rotational speed is being limited proportionally to the change of maximum linear speed, in order to keep current robot behavior untouched.	Yes

在本教程中，我们将使用方法“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”用作特定于插件的参数的命名空间。当我们讨论参数文件（或 params 文件）时，我们将对此有更多了解。

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

在“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”，它是我们的基类。

要导出控制器，我们需要提供两行

#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: Plugin’s library name and its location.
class name: Name of the class.
class type: Type of class.
base class: Name of the base class.
description: Description of the plugin.

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

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

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>

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

3- 通过 params 文件传递插件名称

要启用该插件，我们需要修改“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- 运行 Pure Pursuit Controller 插件

运行启用 Nav2 的 Turtlebot3 模拟。有关如何运行的详细说明，请参阅：ref:getting_started。下面是快捷命令：

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

然后转到 RViz，单击顶部的“2D 姿势估计”按钮，并按照入门指南中所述指向地图上的位置。机器人将在地图上定位，然后单击“Nav2 目标”，然后单击您希望机器人导航到的姿势。之后，控制器将使机器人遵循全局路径。