|
|
|
|
|
|
🧩 Waypoint Navigation Plugins#
In this section, you’ll learn which are the available Waypoint Actions Plugins and how to create new ones. This will allow you to define new behaviors that the robot can perform upon reaching each Waypoint.
In addition to the Nav2 Waypoint Navigation Plugins the available plugins in PAL Navigation for ROS2 package allow to:
Spin in place at a waypoint;
Set parameters at a waypoint;
Set navigation configurations at a waypoint.
Each one of these plugins has a list of parameters that can be configured according to the action you want to perform. They are Task Executors , meaning, they define the behavior to be performed by the robot upon the arrival at a waypoint. The node that handles the behavior execution is the behavior_server.
The behaviors implemented are listed in the pal_nav2_behaviors package which is an extension of
the nav2_behaviors package.
Instruction on how to create a new behavior plugin can be found in the official documentation.
These plugins can be dynamically loaded by Behavior Tree Action Nodes,
with the ProcessAtWaypoint action plugin.
Note
Remember that these plugins can be implemented by the Waypoint Follower and the PAL NavigateThroughWaypoints navigation logics.
How to create your Waypoint Navigation Plugin#
If you want the robot to perform your custom action at the waypoint arrival, you should create a new Task Executor plugin.
Let’s see an example of a Task Executor plugin that makes the robot print a message at the waypoint arrival:
#include <string>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/waypoint_task_executor.hpp"
#include "nav2_util/node_utils.hpp"
namespace waypoint_navigation_tutorial
{
class DummyActionAtWaypoint : public nav2_core::WaypointTaskExecutor
{
private:
bool is_enabled_;
rclcpp::Logger logger_{ rclcpp::get_logger("dummy_action_at_waypoint") };
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
public:
DummyActionAtWaypoint()
{
is_enabled_ = true;
}
~DummyActionAtWaypoint() = default;
void initialize(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent,
const std::string& plugin_name) override
{
parent_ = parent;
auto node = parent_.lock();
if (!node)
{
throw std::runtime_error{ "Failed to lock node in dummy action at waypoint plugin!" };
}
logger_ = node->get_logger();
nav2_util::declare_parameter_if_not_declared
(node, plugin_name + ".enabled", rclcpp::ParameterValue(is_enabled_));
node->get_parameter(plugin_name + ".enabled", is_enabled_);
}
bool processAtWaypoint(const geometry_msgs::msg::PoseStamped& ,
const int& curr_waypoint_index) override
{
if (!is_enabled_)
{
RCLCPP_INFO(logger_, "DummyActionAtWaypoint is disabled");
return true;
}
auto node = parent_.lock();
if (!node)
{
throw std::runtime_error{ "Failed to lock node in dummy action at waypoint plugin!" };
}
// Here you can implement your custom action
RCLCPP_INFO(logger_, "Arrived at %i'th waypoint", curr_waypoint_index);
return true;
}
};
} // namespace waypoint_navigation_tutorial
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(waypoint_navigation_tutorial::DummyActionAtWaypoint,
nav2_core::WaypointTaskExecutor)
Examine the code#
First, you need to include the necessary headers and namespaces.
Then, you should create a class that inherits from the
nav2_core::WaypointTaskExecutor class.
class DummyActionAtWaypoint : public nav2_core::WaypointTaskExecutor
This class provides the following virtual methods that need to be implemented by the plugin class.
Virtual method |
Method description |
Requires override? |
initialize() |
Method that is called when the Waypoint Navigation plugin is initialized.
It is used to setup the plugin with all its publishers and subscribers, or
any ROS service that will be used in the plugin. This method takes in input
a |
Yes |
processAtWaypoint() |
Method that is used to define the body of the task that would be executed
once the robot arrives to a waypoint. This method takes two inputs.
A |
Yes |
In the initialize() function you can setup a logger and a pointer to the parent node of the plugin.
rclcpp::Logger logger_{rclcpp::get_logger("dummy_action_at_waypoint")};
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
The logger_ is an instance of the rclcpp::Logger class provided by the rclcpp package,
which is the ROS 2 client library for C++. This class is used for logging messages.
Meanwhile, parent_ is an instance of the rclcpp_lifecycle::LifecycleNode::WeakPtr class,
needed inside this plugin to point to the base class of the plugin, that’s to say the
nav2_core::WaypointTaskExecutor class.
The last two lines are needed to export the plugin to the pluginlib system.
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(pal_nav2_waypoint_follower::DummyActionAtWaypoint, nav2_core::WaypointTaskExecutor)
Compile and install#
Once you have your plugin, you should export it in order to be able to use it in your waypoint navigation application.
To do so, you should create a CMakeLists.txt file with the following (minimum) dependencies:
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_util REQUIRED)
find_package(pluginlib REQUIRED)
set(dependencies
rclcpp
rclcpp_lifecycle
geometry_msgs
nav2_core
nav2_util
pluginlib
)
To compile and install the plugin, you need to add and install a new library
add_library(dummy_action_at_waypoint SHARED src/dummy_action_at_waypoint.cpp)
ament_target_dependencies(dummy_action_at_waypoint ${dependencies})
install(TARGETS dummy_action_at_waypoint
DESTINATION lib/${PROJECT_NAME})
Finally, you need to export the newly created library and its dependencies
ament_export_libraries(dummy_action_at_waypoint)
ament_export_dependencies(${dependencies})
pluginlib_export_plugin_description_file(waypoint_navigation_tutorial plugins.xml)
ament_package()
Also, within your package, it should be created a new XML file called plugins.xml that contains the plugin
description.
<class_libraries>
<library path="dummy_action_at_waypoint">
<class type="waypoint_navigation_tutorial::DummyActionAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
<description>Prints a message in the log upon the waypoint arrival</description>
</class>
</library>
</class_libraries>
In the package.xml file, you should include the required dependencies:
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>geometry_msgs</depend>
<depend>nav2_core</depend>
<depend>nav2_util</depend>
<depend>pluginlib</depend>
And you should export the plugin
<export>
<build_type>ament_cmake</build_type>
<waypoint_navigation_tutorial plugin="${prefix}/plugins.xml" />
</export>
Usage#
.pal directory of your robot.mkdir ~/.pal/pal_nav2_bt_navigator/params
and then, within this folder, you can create a new configuration file:
touch ~/.pal/pal_nav2_bt_navigator/params/my_navigate_through_waypoints.yaml
Configure your file following the guide in the 📔 Navigation Configuration Guide section and
then add a new entry to the process_at_waypoint_plugins:
sudo vi ~/.pal/pal_nav2_bt_navigator/params/my_navigate_through_waypoints.yaml
__node_name__:
ros__parameters:
# Other pal_nav2_bt_navigator parameters
process_at_waypoint_plugins:
# Other Waypoint Task Executor plugins
- dummy_action_at_waypoint
dummy_action_at_waypoint:
package: "waypoint_navigation_tutorial"
plugin: "pal_nav2_waypoint_follower::DummyActionAtWaypoint"
# Other Waypoint Task Executor plugin configurations