README.md

Exercise 2 - Add Custom Plugin

A plugin provides a concrete implementation for the navigation core, extending it with planners, controllers, map managers, and localizers, compatible with ROS 2.

Each plugin resides in its own ROS 2 package and is registered through pluginlib, enabling dynamic loading at runtime.

Developing Your Own Plugin

To develop your own plugin, you simply need to inherit from the appropriate base class, such as LocalizerMethodBase, PlannerMethodBase, or ControllerMethodBase, MapsManagerMethodBase. Depending on the type and its specific requirements, each plugin must implement either the internal_update_rt() or internal_update() function, or both.

Within these functions, the developer can define what information to read from or write to the NavState blackboard. This design provides complete flexibility to implement custom control logic and share data between modules as needed.

The NavState Blackboard

NavState implements a shared blackboard pattern that serves as the central data hub for all EasyNav modules. It enables modules to exchange information directly without relying on internal ROS 2 communication mechanisms.

All information used or generated by EasyNav modules passes through NavState. Typical examples include:

• The robot’s estimated pose: robot_pose
• A list of navigation goals: goals
• The planned path to follow: path
• Environment maps: map, map.static, map.dynamic
• Control commands: cmd_vel

Implementation of a Custom Plugin

Let’s develop our own Planner plugin. To begin, define a class that inherits from PlannerMethodBase, the base class for all planners in EasyNav.

class WorkshopPlanner : public PlannerMethodBase
{
public:
  explicit WorkshopPlanner();
  virtual std::expected<void, std::string> on_initialize() override;
  void update(NavState & nav_state) override;

  nav_msgs::msg::Path create_linear_path(
            const geometry_msgs::msg::Pose & robot_pose,
            const geometry_msgs::msg::Pose & goal_pose,
            const std::string & frame_id);
protected:
  ...
}

In the on_initialize() function, you can declare custom parameters and create publishers or subscribers as needed. For example, a planner typically creates a publisher to output the generated path (nav_msgs::msg::Path), which can be visualized in RViz.

In this example, we implement a helper function called create_linear_path(), which generates a straight-line path from the robot’s current position to the goal. The core of the plugin logic resides in the update() function which is called periodically by the EasyNav framework to update the navigation state.

1. Checking for Required Data

Before performing any computation, the plugin should verify that all the required inputs exist in the NavState. If the robot’s pose or the list of navigation goals is missing, the planner should safely return without doing anything:

if (!nav_state.has("robot_pose") || !nav_state.has("goals")) {
  RCLCPP_DEBUG(get_node()->get_logger(), "Missing robot_pose or goals. Skipping update.");
  return;
}

This pattern ensures that the plugin only runs when the necessary information is available.

2. Reading Data from the Blackboard

All information in EasyNav is stored in the NavState blackboard. You can retrieve any value using the templated get() method:

const auto & robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
const auto & goal_pose = goals.goals.front();

Once you have the robot’s current pose and at least one goal, you can start generating a path.

3. Generating a Path

The create_linear_path() function is a simple example of how to create a path. It linearly interpolates between the robot’s current position and the goal position, producing a smooth list of waypoints that form a straight line.

current_path_ = create_linear_path(robot_pose.pose.pose, goals.goals.front().pose, "map");

In a real-world planner, this could be replaced by a more advanced algorithm (e.g., RRT, A*, or a costmap-based planner).

5. Writing Results Back to NavState

The planner then stores the computed path and related metadata in the shared blackboard, so other modules, such as the controller, can access it:

nav_state.set("path", current_path_);

Custom type printers

Another powerful feature of NavState is its support for custom printers, which allow you to define how user-defined data types are displayed in debug output. In this example, we register a printer for the PathInfo type to produce readable, structured diagnostic information.

Within the class constructor, we define a printer that formats how PathInfo objects appear when printed from the NavState blackboard. PathInfo is a simple structure containing the origin, goal, and number of waypoints of a generated path.

NavState::register_printer<PathInfo>(
  [](const PathInfo &value)
  {
    std::ostringstream oss;
    oss << "Generated linear path:\n"
        << "  Origin: (" << std::fixed << std::setprecision(2)
        << value.origin.x << ", " << value.origin.y << ", " << value.origin.z << ")\n"
        << "  Radius: " << std::fixed << std::setprecision(2) << value.radius << " m\n"
        << "  Waypoints: " << value.num_waypoints;
    return oss.str();
  });

This printer is automatically used when logging NavState contents, providing formatted output for debugging and monitoring.

After generating the path, we store the associated metadata in the blackboard, allowing other modules to access it and providing formatted debug information for EasyNav tools such as the CLI or TUI.

nav_state.set("path_info", path_info);

Register the Plugin

At the end of your plugin source file, you need to register your plugin so that EasyNav can load it:

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(easynav::WorkshopPlanner, easynav::PlannerMethodBase)

PLUGINLIB_EXPORT_CLASS is a macro provided by pluginlib that exposes your class as a loadable plugin. The first argument (easynav::WorkshopPlanner) is the class implemented, and the second, (easynav::PlannerMethodBase) is the base class the plugin inherits from.

Once registered, EasyNav can discover and load your custom plugin using its name and configuration parameters.

Add you plugin to your config file

After completing your custom plugin in EasyNav, you need to declare it in the configuration YAML file. This informs the system which planner types are available and specifies how to load your plugin along with its parameters.

Below is an example of how to add your plugin to the configuration:

planner_node:
  ros__parameters:
    use_sim_time: true
    planner_types: [<your_type>]  # List all planner types you want to use
    <your_type>:                  # This key should match the type in planner_types
      plugin: <your_registered_plugin>  # The name you registered your plugin with
      <plugin_parameter_1>: <value_1>   # Any parameters your plugin needs
      <plugin_parameter_2>: <value_2>
      # Add more parameters as needed

This setup ensures your custom plugin is loaded and configured with the parameters you need for your navigation scenario.

For this example you need to change the previous configuration file with:

planner_node:
  ros__parameters:
    use_sim_time: true
    planner_types: [workshop_planner]
    workshop_planner:
      plugin: easynav_workshop_planner/WorkshopPlanner
      path_wp: 13

Execution

To test our custom plugin (located at ~/workshop_ws/src/exercises/easynav/easynav_playground/easynav_workshop_planner), follow these steps:

1. Launch the Kobuki playground. We can use it without graphic interface:

ros2 launch easynav_playground_kobuki playground_kobuki.launch.py gui:=false

2. Start EasyNav with this exercise params:

ros2 run easynav_system system_main --ros-args --params-file ~/workshop_ws/src/exercises/easynav/easynav_playground/easynav_workshop_testcase/config/custom.params.yaml 

3. Start RViz (use simulation time):

ros2 run rviz2 rviz2 -d ~/workshop_ws/src/exercises/easynav/easynav_playground/easynav_workshop_testcase/rviz/costmap.rviz 

Check all information from the path is provided both in the logger of the easynav_system terminal and the TUI.