编写 navigation2 控制器插件

简介

  本教程展示了如何创建自己的控制器插件。在本教程中，我们将基于这篇论文实现纯追踪路径跟踪算法。建议您阅读该论文。
  注意：本教程基于 Nav2 堆栈中以前存在的简化版本的 Regulated Pure Pursuit 控制器。您可以在此处找到与本教程相匹配的源代码。

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、setPlan 和 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” 获取了名为 desired_linear_vel 的ROS 参数，该参数是特定于我们的控制器的。Nav2 允许加载多个插件，并且为了保持事物的组织性，每个插件都映射到某个ID/名称。现在，如果我们想要检索特定插件的参数，我们可以使用.，就像上面的片段中所做的那样。例如，我们的示例控制器被映射到名称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 的派生类。

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

• 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_cmakebuild_type>
  <nav2_core plugin="${prefix}/pure_pursuit_controller_plugin.xml" />
export>

• 5.编译后，它应该被注册了。接下来，我们将使用这个插件。

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

要启用该插件，我们需要修改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。为了传递插件特定的参数，我们使用了.的格式。

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

  使用启用了 Nav2 的 Turtlebot3 仿真。如何运行它的详细说明写在了Getting Started中。下面是一个快捷命令:

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

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