action_based_executor_node.hpp

// Copyright 2026 Robin Müller
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#pragma once
 
#include <map>
 
#include "auto_apms_behavior_tree/executor/generic_executor_node.hpp"
#include "auto_apms_util/action_context.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
 
namespace auto_apms_behavior_tree
{

template <typename ActionT>
class ActionBasedTreeExecutorNode : public GenericTreeExecutorNode
{
public:
  using TriggerActionContext = auto_apms_util::ActionContext<ActionT>;
  using TriggerGoal = typename ActionT::Goal;
  using TriggerFeedback = typename ActionT::Feedback;
  using TriggerResult = typename ActionT::Result;
  using TriggerGoalHandle = rclcpp_action::ServerGoalHandle<ActionT>;

  ActionBasedTreeExecutorNode(rclcpp::Node::SharedPtr node_ptr, const std::string & action_name, Options options);

  ActionBasedTreeExecutorNode(const std::string & name, const std::string & action_name, Options options);

  ActionBasedTreeExecutorNode(const std::string & name, const std::string & action_name);
 
  virtual ~ActionBasedTreeExecutorNode() override = default;
 
protected:
  /* Virtual methods to implement/override */

  virtual TreeConstructor getTreeConstructorFromGoal(std::shared_ptr<const TriggerGoal> goal_ptr) = 0;

  virtual bool shouldAcceptGoal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const TriggerGoal> goal_ptr);

  virtual void onAcceptedGoal(std::shared_ptr<TriggerGoalHandle> goal_handle_ptr);

  virtual void onExecutionStarted(std::shared_ptr<TriggerGoalHandle> goal_handle_ptr);

  virtual void onGoalExecutionTermination(const ExecutionResult & result, TriggerActionContext & context);
 
  TriggerActionContext trigger_action_context_;
  std::map<rclcpp_action::GoalUUID, TreeConstructor> pending_tree_constructors_;
 
private:
  void onTermination(const ExecutionResult & result) override final;
 
  /* Internal action callbacks */
 
  rclcpp_action::GoalResponse handle_trigger_goal_(
    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const TriggerGoal> goal_ptr);
  rclcpp_action::CancelResponse handle_trigger_cancel_(std::shared_ptr<TriggerGoalHandle> goal_handle_ptr);
  void handle_trigger_accept_(std::shared_ptr<TriggerGoalHandle> goal_handle_ptr);
 
  typename rclcpp_action::Server<ActionT>::SharedPtr trigger_action_ptr_;
};
 
// #####################################################################################################################
// ################################              DEFINITIONS              ##############################################
// #####################################################################################################################
 
template <typename ActionT>
ActionBasedTreeExecutorNode<ActionT>::ActionBasedTreeExecutorNode(
  rclcpp::Node::SharedPtr node_ptr, const std::string & action_name, Options options)
: GenericTreeExecutorNode(node_ptr, options), trigger_action_context_(logger_)
{
  using namespace std::placeholders;
  trigger_action_ptr_ = rclcpp_action::create_server<ActionT>(
    node_ptr_, action_name, std::bind(&ActionBasedTreeExecutorNode::handle_trigger_goal_, this, _1, _2),
    std::bind(&ActionBasedTreeExecutorNode::handle_trigger_cancel_, this, _1),
    std::bind(&ActionBasedTreeExecutorNode::handle_trigger_accept_, this, _1));
}
 
template <typename ActionT>
ActionBasedTreeExecutorNode<ActionT>::ActionBasedTreeExecutorNode(
  const std::string & name, const std::string & action_name, Options options)
: ActionBasedTreeExecutorNode(std::make_shared<rclcpp::Node>(name, options.getROSNodeOptions()), action_name, options)
{
}
 
template <typename ActionT>
ActionBasedTreeExecutorNode<ActionT>::ActionBasedTreeExecutorNode(
  const std::string & name, const std::string & action_name)
: ActionBasedTreeExecutorNode(name, action_name, Options(rclcpp::NodeOptions()))
{
}
 
template <typename ActionT>
bool ActionBasedTreeExecutorNode<ActionT>::shouldAcceptGoal(
  const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const TriggerGoal> /*goal_ptr*/)
{
  if (isBusy()) {
    RCLCPP_WARN(
      logger_, "Goal %s was REJECTED: Tree '%s' is currently executing.", rclcpp_action::to_string(uuid).c_str(),
      getTreeName().c_str());
    return false;
  }
  return true;
}
 
template <typename ActionT>
void ActionBasedTreeExecutorNode<ActionT>::onAcceptedGoal(std::shared_ptr<TriggerGoalHandle> /*goal_handle_ptr*/)
{
}
 
template <typename ActionT>
void ActionBasedTreeExecutorNode<ActionT>::onExecutionStarted(std::shared_ptr<TriggerGoalHandle> goal_handle_ptr)
{
  trigger_action_context_.setUp(goal_handle_ptr);
  RCLCPP_INFO(logger_, "Successfully started execution of tree '%s' via action trigger.", getTreeName().c_str());
}
 
template <typename ActionT>
void ActionBasedTreeExecutorNode<ActionT>::onGoalExecutionTermination(
  const ExecutionResult & result, TriggerActionContext & context)
{
  switch (result) {
    case ExecutionResult::TREE_SUCCEEDED:
      context.succeed();
      break;
    case ExecutionResult::TREE_FAILED:
      context.abort();
      break;
    case ExecutionResult::TERMINATED_PREMATURELY:
      if (context.getGoalHandlePtr()->is_canceling()) {
        context.cancel();
      } else {
        context.abort();
      }
      break;
    case ExecutionResult::ERROR:
    default:
      context.abort();
      break;
  }
}
 
template <typename ActionT>
void ActionBasedTreeExecutorNode<ActionT>::onTermination(const ExecutionResult & result)
{
  if (trigger_action_context_.isValid()) {
    onGoalExecutionTermination(result, trigger_action_context_);
    trigger_action_context_.invalidate();
  }
}
 
template <typename ActionT>
rclcpp_action::GoalResponse ActionBasedTreeExecutorNode<ActionT>::handle_trigger_goal_(
  const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const TriggerGoal> goal_ptr)
{
  if (!shouldAcceptGoal(uuid, goal_ptr)) {
    return rclcpp_action::GoalResponse::REJECT;
  }
 
  try {
    pending_tree_constructors_[uuid] = getTreeConstructorFromGoal(goal_ptr);
  } catch (const std::exception & e) {
    RCLCPP_WARN(
      logger_, "Goal %s was REJECTED: Exception in getTreeConstructorFromGoal(): %s",
      rclcpp_action::to_string(uuid).c_str(), e.what());
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
 
template <typename ActionT>
rclcpp_action::CancelResponse ActionBasedTreeExecutorNode<ActionT>::handle_trigger_cancel_(
  std::shared_ptr<TriggerGoalHandle> /*goal_handle_ptr*/)
{
  setControlCommand(ControlCommand::TERMINATE);
  return rclcpp_action::CancelResponse::ACCEPT;
}
 
template <typename ActionT>
void ActionBasedTreeExecutorNode<ActionT>::handle_trigger_accept_(std::shared_ptr<TriggerGoalHandle> goal_handle_ptr)
{
  onAcceptedGoal(goal_handle_ptr);
 
  const rclcpp_action::GoalUUID uuid = goal_handle_ptr->get_goal_id();
  auto node = pending_tree_constructors_.extract(uuid);
  if (node.empty()) {
    RCLCPP_ERROR(logger_, "No pending tree constructor found for goal %s.", rclcpp_action::to_string(uuid).c_str());
    auto result_ptr = std::make_shared<TriggerResult>();
    goal_handle_ptr->abort(result_ptr);
    return;
  }
  TreeConstructor tree_constructor = std::move(node.mapped());
 
  const ExecutorParameters params = executor_param_listener_.get_params();
  try {
    startExecution(tree_constructor, params.tick_rate, params.groot2_port);
  } catch (const std::exception & e) {
    auto result_ptr = std::make_shared<TriggerResult>();
    goal_handle_ptr->abort(result_ptr);
    RCLCPP_ERROR(logger_, "An error occurred trying to start execution: %s", e.what());
    return;
  }
 
  onExecutionStarted(goal_handle_ptr);
}
 
}  // namespace auto_apms_behavior_tree