ROS2与Navigation2入门教程-编写新的控制器（Controller）插件

说明：

• 介绍如何编写新的控制器（Controller）插件

概述

• 本教程将会说明如何创建自己的控制器插件

• 在本教程中，将会基于这篇论文实现纯追踪路径跟踪算法。建议通读该论文

• 本教程是基于Nav2软件堆栈中受管制纯追踪（Regulated Pure Pursuit）控制器的一个现存简化版之上的。

• 可以在这里找到与本教程相匹配的源代码。

要求

• 要求在本地机器上已经安装或构建好了以下软件包：
  • ROS 2（二进制安装或从源代码构建）
  • Nav2（包括依赖包）
  • Gazebo
  • Turtlebot3

具体步骤

• 创建一个新的控制器插件

• 本教程中将会实现纯追踪控制器。本教程中的注释代码可以在navigation_tutorials资源库的nav2_pure_pursuit_controller软件包中找到。

• 这个软件包可以作为编写自定义控制器插件的参考。

• 本示例插件类nav2_pure_pursuit_controller::PurePursuitController继承自基类nav2_core::Controller。

• 该基类提供了一系列虚拟方法来实现控制器插件。控制器服务器会在运行时调用这些方法来计算速度指令。

• 这些方法的列表、描述和必要性列于下表：

• 本教程将会使用PurePursuitController::computeVelocityCommands、PurePursuitController::configure和PurePursuitController::setPlan方法。

• 在控制器中，configure()方法必须从ROS参数设置成员变量并执行任何所需的初始化

• 如下所示：

void PurePursuitController::configure(
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
  std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
  const 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()方法中会计算期望的速度。使用该方法来计算给定当前速度和位姿情况下的期望速度指令。

• 在纯追踪情况下，该算法会计算速度指令，以便机器人尽可能完全遵循全局路径。

• 该算法假设线速度恒定，并根据全局路径的曲率计算角速度。

geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands(
  const geometry_msgs::msg::PoseStamped & pose,
  const geometry_msgs::msg::Twist & velocity)
{
  // Find the first pose which is at a distance greater than the specified lookahed 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;
}

• 其余方法没有使用，但必须覆盖它们。根据规则，这里确实会覆盖所有这些方法，但这些方法均为空的。

导出控制器插件

• 现在已经创建了自定义控制器，需要导出该控制器插件以便控制器服务器可以看见该插件。

• 插件在运行时加载，但如果插件不可见，则控制器服务器将无法加载该插件。在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文件。

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

• 下一步是通过在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" />

• 编译该插件软件包，这样应该就完成了该插件的注册。下一步就可以使用该插件了。

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

• 要启用该插件，需要如下所示修改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>。

运行纯追踪控制器（Pure Pursuit Controller）插件

• 运行带有已启用Nav2的Turtlebot3机器人仿真。有关如何启用Nav2的详细说明请参阅“开始使用Nav2”教程。
• 下面是运行该仿真的快捷命令：

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

• 然后进入到RViz中并单击顶部的“2D Pose Estimate”按钮，并按照“开始使用Nav2”教程所介绍的那样在地图上点击机器人的初始位置。机器人将会在地图上定位，然后点击“Navigation2 goal”按钮，并在您想要让机器人导航到的位置上单击。

• 这样，控制器就会控制机器人遵循全局路径进行导航。

参考：

• https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html