Introduction
This tutorial shows how to create your own controller plugin. In this tutorial, we will implement a pure path tracking algorithm based on this paper. It is recommended that you read the paper.
Note: This tutorial is based on a simplified version of the Regulated Pure Pursuit controller that previously existed in the Nav2 stack. You can find the source code that matches this tutorial here.
1- Create a new controller plugin
We will implement a pure tracking controller. The annotated code in this tutorial can be found in nav2_pure_pursuit_controller in the navigation_tutorials repository. This package can be considered a reference for writing your own controller plugins.
Our example plugin class nav2_pure_pursuit_controller::PurePursuitController inherits from the base class nav2_core::Controller. The base class provides a set of virtual methods to implement controller plugins. These methods are called by the controller server at runtime to calculate velocity commands. The following table lists the various virtual functions, function descriptions and whether they need to be rewritten:
| Virtual function | Method description | Need to be rewritten? |
|---|---|---|
| configure() | This method is called when the controller server enters the on_configure state. Ideally, this method should perform the declaration of ROS parameters and initialization of controller member variables. This method accepts 4 input parameters: the weak reference of the parent node, the controller name, the tf buffer pointer and the shared pointer of the costmap. | Yes |
| activate() | This method is called when the controller server enters the on_activate state. Ideally, this method should implement the necessary actions before the controller becomes active. | Yes |
| deactivate() | This method is called when the controller server enters the on_deactivate state. Ideally, this method should implement the necessary actions before the controller enters the inactive state. | Yes |
| cleanup() | This method is called when the controller server enters the on_cleanup state. Ideally, this method should clean up the resources created for the controller. | Yes |
| setPlan() | This method is called when the global path is updated. Ideally, this method should perform the operation of converting the global path and the storage path. | Yes |
| computeVelocityCommands() | When the controller server asks for new velocity commands so that the robot follows the global path , call this method. This method returns a geometry_msgs::msg::TwistStamped, which represents the speed command of the robot. This method is passed two parameters: a reference to the current robot pose and the current velocity. | Yes |
| setSpeedLimit() | This method is called when it is necessary to limit the maximum linear speed of the robot. Speed limits can be expressed as absolute values (m/s) or as a percentage relative to the maximum robot speed. Note that typically the maximum rotational speed is limited in proportion to the change in maximum linear speed to keep the current robot behavior unchanged. | Yes |
In this tutorial, we will use the configure, setPlan, and computeVelocityCommands methods of the PurePursuitController class.
In the controller, the configure() method must set the member variables from the ROS parameters and perform any required initialization operations.
void PurePursuitController::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & amp; 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);
}
Here, plugin_name_ + “.desired_linear_vel” gets the ROS parameter named desired_linear_vel which is specific to our controller. Nav2 allows multiple plugins to be loaded, and to keep things organized, each plugin is mapped to a certain ID/name. Now, if we want to retrieve the parameters of a specific plugin, we can use
The parameters passed in are stored in member variables so that they can be used at a later stage if needed.
In the setPlan() method, we receive the updated global path that the robot needs to follow. In our example, we convert the received global path into the robot’s coordinate system and store this converted global path for later use.
void PurePursuitController::setPlan(const nav_msgs::msg::Path & amp; path)
{<!-- -->
// Transform global path into the robot's frame
global_plan_ = transformGlobalPlan(path);
}
The operation of calculating the expected velocity occurs in the computeVelocityCommands() method. It is used to calculate the desired speed command based on the current speed and attitude. The third parameter is a pointer to nav2_core::GoalChecker, used to check whether the goal has been reached. In our example, this parameter will not be used. For a pure tracking algorithm, the algorithm calculates velocity commands so that the robot follows the global path as closely as possible. The algorithm assumes that the linear velocity is constant and calculates the angular velocity based on the curvature of the global path.
geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & amp; pose,
const geometry_msgs::msg::Twist & amp; 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(),
[ & amp;](const auto & amp; 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;
}
Although the remaining methods are not used, they must be rewritten. As per regulations, we do do the overrides, but leave them empty.
2- Export controller plug-in
Now that we have created our custom controller, we need to export our controller plugin so it can be seen by the controller server. Plugins are loaded at runtime, if they are not visible then our controller server cannot load them. In ROS 2, exporting and loading plugins is handled by pluginlib.
Going back to our tutorial, the nav2_pure_pursuit_controller::PurePursuitController class is loaded dynamically as a derived class of nav2_core::Controller.
- 1. To export this controller, we need to provide two lines of code
#include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller)
Please note that it requires pluginlib to export the plugin’s classes. Pluginlib will provide the macro PLUGINLIB_EXPORT_CLASS, which is responsible for completing the export work.
It’s good practice to put these two lines of code at the end of the file, but technically you could also put them at the top of the file.
- 2. The next step is to create the plugin’s description file in the root directory of the package. For example, create the
pure_pursuit_controller_plugin.xmlfile in our tutorial package. The file contains the following information:
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. The next step is to export the plug-in using
CMakeLists.txt, using the CMake functionpluginlib_export_plugin_description_file(). This function installs the plugin description file into the share directory and sets the ament index to make it discoverable.
pluginlib_export_plugin_description_file(nav2_core pure_pursuit_controller_plugin.xml)
- 4 The plug-in description file should also be added to
package.xml.
<export>
<build_type>ament_cmake</build_type>
<nav2_core plugin="${prefix}/pure_pursuit_controller_plugin.xml" />
</export>
- 5. After compilation, it should be registered. Next, we will use this plugin.
3- Pass the plugin name through params file
To enable the plugin, we need to modify the nav2_params.yaml file as follows:
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
In the above code snippet, you can see that we map the nav2_pure_pursuit_controller/PurePursuitController controller to its id FollowPath. To pass plugin-specific parameters, we use the format
4- Run pure tracking controller plug-in
Using Turtlebot3 emulation with Nav2 enabled. Detailed instructions on how to run it are written in Getting Started. Here is a shortcut command:
ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/path/to/your_params_file.yaml
Then go to RViz, click the “2D Pose Estimation” button at the top, and point to the location on the map as described in the “Getting Started Guide”. The robot will be positioned on the map, then click on “Nav2 Target” and click on the pose you want the robot to navigate to. Afterwards, the controller will make the robot follow the global path.