Premise

The version of ROS is Humble. I'm trying to implement a behavior tree plugin for an action that moves an omnidirectional robot in 2D mantaining a fixed direction. I'd like to extend a navigation2 move_to_pose_with_replanning_and_recovery behaviour tree in such a way that the last approach is executed using my custom action. The action works normally when called by an action client or with a goal action message from CLI.

I'm fairly new with C++ but I think I understand most of the nav2_behavior_tree source code.

I created the two files of the wait action example in a separate library, one contains the .hpp code for the plugin, one the .cpp code. The library has the same setup as the nav2_behavior_tree:

- planar_move
  -planar_move
    - ... (action definition code)
  - planar_node (where the bt plugin is defined)
    -include/plugins
      -planar_move_action.hpp
    -plugins
      -planar_move_action.hpp
    -cmake
    -plugin_palette.xml (defines the custom action plugin)
    -custom_bt.xml (defines the custom behavior tree to use with nav to pose)
    -package.xml

this is the planar_move_aciton.cpp code that implements the behavior tree plugin:

#include <memory>
#include <string>

#include "planar_move_interfaces/plugins/action/planar_move.hpp"

namespace its_behaviour_tree
{

PlanarMoveActionNode::PlanarMoveActionNode(
  const std::string & xml_tag_name,
  const std::string & action_name,
  const BT::NodeConfiguration & conf)
: BtActionNode<Action>(xml_tag_name, action_name, conf)
{
}

void PlanarMoveActionNode::on_tick()
{
  if (!getInput("goal", goal_.pose)) {
    RCLCPP_ERROR(
      node_->get_logger(),
      "NavigateToPoseAction: goal not provided");
    return;
  }
}

BT::NodeStatus PlanarMoveActionNode::on_success()
{
  // Set empty error code, action was successful
  setOutput("error_code_id", ActionGoal::NONE);
  return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus PlanarMoveActionNode::on_aborted()
{
  setOutput("error_code_id", result_.result->error_code);
  return BT::NodeStatus::FAILURE;
}

BT::NodeStatus PlanarMoveActionNode::on_cancelled()
{

  setOutput("error_code_id", ActionGoal::NONE);
  return BT::NodeStatus::SUCCESS;
}

}  // namespace its_behaviour_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<its_behaviour_tree::PlanarMoveActionNode>(
        name, "planar_move", config);
    };

  factory.registerBuilder<its_behaviour_tree::PlanarMoveActionNode>(
    "PlanarMove", builder);
}

This it the planar_move_action.hpp code:

#ifndef PLANAR_MOVE_ACTION_HPP_
#define PLANAR_MOVE_ACTION_HPP_

#include <string>

#include "planar_move_interfaces/action/planar_move.hpp"
#include "nav_msgs/msg/path.h"
#include "nav2_behavior_tree/bt_action_node.hpp"

class PlanarMoveActionNode : public BtActionNode<planar_move_interfaces::action::PlanarMove>
{
  using Action = nav2_msgs::action::PlanarMove;
  using ActionResult = Action::Result;
  using ActionGoal = Action::Goal;

public:
  /**
   * @brief A constructor for nav2_behavior_tree::PlanarMoveActionNode
   * @param xml_tag_name Name for the XML tag for this node
   * @param action_name Action name this node creates a client for
   * @param conf BT node configuration
   */
  PlanarMoveActionNode(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf);

  /**
   * @brief Function to perform some user-defined operation on tick
   */
  void on_tick() override;

  /**
   * @brief Function to perform some user-defined operation upon successful completion of the action
   */
  BT::NodeStatus on_success() override;

  /**
   * @brief Function to perform some user-defined operation upon abortion of the action
   */
  BT::NodeStatus on_aborted() override;

  /**
   * @brief Function to perform some user-defined operation upon cancellation of the action
   */
  BT::NodeStatus on_cancelled() override;

  /**
   * @brief Creates list of BT ports
   * @return BT::PortsList Containing basic ports along with node-specific ports
   */
  static BT::PortsList providedPorts()
  {
    return providedBasicPorts(
      {
        BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
      });
  }
};

}  // namespace nav2_behavior_tree

#endif  // PLANAR_MOVE_ACTION_HPP_

Problem

I'm unsure on how to utilize the ... (more)