编写新的控制器插件 [待校准@2100]

• 概述

• 必要条件

• 教程步骤

概述

本教程展示了如何创建自己的控制器 plugin 。 [待校准@2102]

在本教程中，我们将使用单纯的追求路径跟踪算法基于此 论文 。建议你去了解一下。 [校准@小鱼]

注意: 本教程是基于现在Nav2软件包中的受控纯追踪控制器的先前现有简化版本。你可以找到源代码匹配本教程 here 。 [校准@混沌无形]

必要条件

• ROS 2 (二进制或从源代码构建)

• Nav2 (包括依赖项) [待校准@1996]

• Gazebo [待校准@1997]

• Turtlebot3 [待校准@1998]

教程步骤

1-创建一个新的控制器插件 [待校准@2105]

我们将应用纯粹的追踪控制器。本教程中的注解代码在 navigation_tutorials 库的 nav2_pure_pursuit_controller 。这个包可以看作是编写你自己的控制器插件的参考。 [校准@混沌无形]

我们例子插件类 ”nav2_pure_pursuit_controller: PurePursuitController ``继承自基类 `` nav2_core: Controller”。基类提供一组虚拟方法实现控制器插件。调用这些方法在运行控制器服务器计算速度命令。下表列出了各种方法及其说明，以及必要性： [校准@混沌无形]

Virtual method	方法描述	必须重写？
configure()	当控制器服务器进入on_configure状态时调用方法。理想情况下，这个方法应该执行ROS参数的声明和控制器成员变量的初始化。这个方法需要4个输入参数：指向父节点的弱指针、控制器名称、TF缓冲区指针和指向成本地图的共享指针。 [校准@混沌无形]	是 [校准@小鱼]
activate() [待校准@2110]	当控制器服务器进入on_activate状态时调用方法。 理想情况下，这个方法应该在控制器进入活动状态之前实现必要的操作。 [校准@混沌无形]	是 [校准@小鱼]
deactivate()	当控制器服务器进入on_deactivate状态时，方法被调用。理想情况下，这个方法应该实现控制器进入非活动状态前的必要操作。 [校准@混沌无形]	是 [校准@小鱼]
cleanup()	当控制器服务器进入on_clean状态时调用方法。理想情况下，该方法应清理为控制器创建的资源。 [待校准@2115]	是 [校准@小鱼]
setPlan() [待校准@2116]	在更新全局规划时调用方法。理想情况下，该方法应执行转换和存储全局规划的操作。 [校准@混沌无形]	是 [校准@小鱼]
computeVelocityCommands() [待校准@2118]	当控制器服务器要求一个新的速度命令以使机器人跟随全局路径时，该方法被调用。此方法返回 “geometry_msgs::msg::TwistStamped'，表示机器人驱动的速度命令。该方法传递2个参数: 参考当前机器人位姿及其当前速度。 [校准@混沌无形]	是 [校准@小鱼]

在本教程中，我们将使用方法 “PurePursuitController::configure”，“PurePursuitController::setPlan `` 和 `` PurePursuitController::computeVelocityCommands”。 [校准@混沌无形]

在控制器中， 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”的名字，为了检索``desired_linear_vel parameter``，这是特定于"FollowPath”的，我们使用``FollowPath.desired_linear_vel``。换句话说， FollowPath 被用作插件特定参数的命名空间。当我们讨论参数文件 (或params文件) 时，我们将看到更多有关此的信息。 [校准@混沌无形]

传入的参数存储在成员变量中，以便在需要时可以在稍后阶段使用它们。 [待校准@2123]

在 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;
}

其余的方法没有被使用，但必须覆盖它们。按照规定，我们覆盖了所有的方法，但将它们留空。 [校准@混沌无形]

2-导出控制器插件 [校准@混沌无形]

现在我们已经创建了自定义控制器，我们需要导出控制器插件，以便控制器服务器可以看到它。插件是在运行时加载的，如果它们不可见，那么我们的控制器服务器将无法加载它。在ROS 2中，导出和加载插件由 pluginlib 处理。 [待校准@2128]

来到我们的教程， “nav2_pure_pursuit_control:: PurePursuitController `` 类是动态加载的， `` nav2_core::Controller” 是我们的基类。 [校准@混沌无形]

1. 要导出控制器，我们需要提供两行 [待校准@2130]

#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 文件。此文件包含以下信息 [待校准@2133]

• 参数``library path``:插件的库名和它的位置。 [待校准@2134]

• 参数``class name``:类的名称。 [校准@混沌无形]

• 参数``class type``:类的类型 [待校准@2136]

• 参数``base class``:基类的名称。 [待校准@2137]

• 参数``description``:插件的描述。 [待校准@2138]

<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. 下一步是使用CMake函数``pluginlib_export_plugin_description_file()``，用``CMakeLists.txt``导出插件。此函数将插件描述文件安装到 share 目录，并设置属性索引以使其可被发现。 [校准@混沌无形]

pluginlib_export_plugin_description_file(nav2_core pure_pursuit_controller_plugin.xml)

4. 插件描述文件也应添加到 package.xml [待校准@2140]

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

5. 编译，且它应该被注册。接下来，我们将使用这个插件。 [校准@混沌无形]

3-通过params文件传递插件名称 [待校准@2142]

为了启用插件，我们需要修改 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-运行纯追踪控制器插件 [校准@混沌无形]

运行Turtlebot3 仿真启用Nav2。详细说明如何使运行写在 入门 。以下是快捷命令: [待校准@2146]

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

然后转到RViz，点击顶部的 "2D Pose Estimate" 按钮，指出地图上的位置，就像 入门 中描述的那样。机器人将在地图上定位，然后点击 "Nav2 goal" ，点击你希望机器人导航到的姿势。之后，控制器将使机器人跟随全局路径。 [校准@混沌无形]