Error when canceling action during halt()

[MatteoAlbi]

I'm running in a weird behavior when trying to halting an action node.

Setup:
BehaviorTree.ROS2 branch humble bb0d510
BehaviorTree.CPP branch master acbc707

Structure:
ws
|- BehaviorTree.ROS2
|- BehaviorTree.CPP
|- test
|- CMakeLists.txt
|- package.xml
|- src
|- bt_act_test.cpp

CMakeLists.txt:

cmake_minimum_required(VERSION 3.8)
project(bt_act_test)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED )
find_package(rclcpp_action REQUIRED )
find_package(behaviortree_cpp REQUIRED )
find_package(behaviortree_ros2 REQUIRED )
find_package(example_interfaces REQUIRED)

add_executable(bt_act_test src/bt_act_test.cpp)
target_include_directories(bt_act_test PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>
)
ament_target_dependencies(bt_act_test
    rclcpp
    rclcpp_action
    behaviortree_cpp
    behaviortree_ros2
    example_interfaces
)
    

if(BUILD_TESTING)
    find_package(ament_lint_auto REQUIRED)
    # the following line skips the linter which checks for copyrights
    # comment the line when a copyright and license is added to all source files
    set(ament_cmake_copyright_FOUND TRUE)
    # the following line skips cpplint (only works in a git repo)
    # comment the line when this package is in a git repo and when
    # a copyright and license is added to all source files
    set(ament_cmake_cpplint_FOUND TRUE)
    ament_lint_auto_find_test_dependencies()
endif()

install(TARGETS 
    bt_act_test
DESTINATION lib/${PROJECT_NAME})    

ament_package()

package.xml:

...
    <buildtool_depend>ament_cmake</buildtool_depend>

    <depend>rclcpp</depend>
    <depend>rclcpp_action</depend>
    <depend>behaviortree_ros2</depend>
    <depend>behaviortree_cpp</depend>
    <depend>example_interfaces</depend>

    <test_depend>ament_lint_auto</test_depend>
    <test_depend>ament_lint_common</test_depend>

    <export>
      <build_type>ament_cmake</build_type>
    </export>
...

bt_act_test.cpp:

#include <stdio.h>
#include <string>
#include <chrono>
#include <thread>
#include <memory>
#include <filesystem>
#include <vector>

#include <example_interfaces/action/fibonacci.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <behaviortree_ros2/bt_action_node.hpp>

using vec_int = std::vector<int32_t>;
using Fibonacci = example_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;



void sleep(int ms=10){
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}


class FibonacciActionServer : public rclcpp::Node {  

public:

    explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
    : Node("server_node", options)
    {
        using namespace std::placeholders;

        this->action_server_ = rclcpp_action::create_server<Fibonacci>(
            this,
            "fibonacci",
            std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
            std::bind(&FibonacciActionServer::handle_cancel, this, _1),
            std::bind(&FibonacciActionServer::handle_accepted, this, _1)    
        );
    }

private:
    rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const Fibonacci::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
        (void)uuid;
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        using namespace std::placeholders;
        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
        std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        rclcpp::Rate loop_rate(1);
        const auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<Fibonacci::Feedback>();
        auto & sequence = feedback->sequence;
        sequence.push_back(0);
        sequence.push_back(1);
        auto result = std::make_shared<Fibonacci::Result>();

        for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
            // Check if there is a cancel request
            if (goal_handle->is_canceling()) {
                result->sequence = sequence;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                return;
            }
            // Update sequence
            sequence.push_back(sequence[i] + sequence[i - 1]);
            // Publish feedback
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(), "Publish feedback");

            loop_rate.sleep();
        }

        // Check if goal is done
        if (rclcpp::ok()) {
            result->sequence = sequence;
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
        }
    }
};

using namespace BT;

class FibonacciAction: public RosActionNode<Fibonacci>
{
public:
  FibonacciAction(const std::string& name,
                  const NodeConfig& conf,
                  const RosNodeParams& params)
    : RosActionNode<Fibonacci>(name, conf, params)
  {}

  // The specific ports of this Derived class
  // should be merged with the ports of the base class,
  // using RosActionNode::providedBasicPorts()
  static PortsList providedPorts()
  {
    return providedBasicPorts({InputPort<unsigned>("order"), OutputPort<vec_int>("result")});
  }

  // This is called when the TreeNode is ticked and it should
  // send the request to the action server
  bool setGoal(RosActionNode::Goal& goal) override 
  {
    // get "order" from the Input port
    getInput("order", goal.order);
    // return true, if we were able to set the goal correctly.
    return true;
  }
  
  // Callback executed when the reply is received.
  // Based on the reply you may decide to return SUCCESS or FAILURE.
  NodeStatus onResultReceived(const WrappedResult& wr) override
  {
    std::stringstream ss;
    ss << "Result received: ";
    for (auto number : wr.result->sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(node_->get_logger(), ss.str().c_str());
    this->setOutput<vec_int>("result", wr.result->sequence);
    return NodeStatus::SUCCESS;
  }

  // Callback invoked when there was an error at the level
  // of the communication between client and server.
  // This will set the status of the TreeNode to either SUCCESS or FAILURE,
  // based on the return value.
  // If not overridden, it will return FAILURE by default.
  virtual NodeStatus onFailure(ActionNodeErrorCode error) override
  {
    RCLCPP_ERROR(node_->get_logger(), "Error: %d", error);
    return NodeStatus::FAILURE;
  }

  // we also support a callback for the feedback, as in
  // the original tutorial.
  // Usually, this callback should return RUNNING, but you
  // might decide, based on the value of the feedback, to abort
  // the action, and consider the TreeNode completed.
  // In that case, return SUCCESS or FAILURE.
  // The Cancel request will be send automatically to the server.
  NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback)
  {
    std::stringstream ss;
    ss << "Next number in sequence received: ";
    for (auto number : feedback->sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(node_->get_logger(), ss.str().c_str());
    this->setOutput<vec_int>("result", feedback->sequence);
    return NodeStatus::RUNNING;
  }
};

template<typename T>
class GetBBvalueNode : public BT::SyncActionNode{
public:
    GetBBvalueNode (
        const std::string& name, const BT::NodeConfig& config, 
        std::function<BT::NodeStatus(T)> callback,
        std::function<BT::NodeStatus(BT::Expected<T>)> no_value_callback = [](BT::Expected<T> input){
            throw BT::RuntimeError("missing required input [input]: ", input.error() );
            return BT::NodeStatus::FAILURE; //needed to pass contructor checks
        }
    ) : 
    BT::SyncActionNode(name, config),
    _callback(callback),
    _no_value_callback(no_value_callback) {}

    BT::NodeStatus tick() override{
        auto input = getInput<T>("input"); //get value from port "input"

        // Check if expected is valid. If not, throw its error
        if (!input) return this->_no_value_callback(input);
        else return this->_callback(input.value());
    }

    /**
     * @brief set port input as <T> input
    */
    static BT::PortsList providedPorts(){
        return{ BT::InputPort<T>("input", "read value")};
    }

private:
    std::function<BT::NodeStatus(T)> _callback;
    std::function<BT::NodeStatus(BT::Expected<T>)> _no_value_callback;
};

int main(int argc, char *argv[]){
    rclcpp::init(argc, argv);

    static const char* xml_text = R"(
    <root BTCPP_format="4" >
        <BehaviorTree ID="MainTree">
            <ReactiveSequence>
                <GetVec input="{res}"/>
                <Fibonacci  order="5"
                            result="{res}"/>
            </ReactiveSequence>
        </BehaviorTree>
    </root>
    )";
    
    std::vector<vec_int> res_vecs;

    auto get_vec = [&res_vecs](vec_int input){
        if ( std::find(res_vecs.begin(), res_vecs.end(), input) == res_vecs.end() ){ 
            res_vecs.push_back(input);
            for(auto el : res_vecs.back()) std::cout << el << "; ";
            std::cout << std::endl;
        } 
        if (res_vecs.size() == 2) return BT::NodeStatus::FAILURE;
        else return BT::NodeStatus::SUCCESS;
    };
    auto on_err = [](BT::Expected<vec_int> input){
        (void)input; 
        return BT::NodeStatus::SUCCESS;
    };


    BehaviorTreeFactory factory;
    // provide the ROS node and the name of the action service
    RosNodeParams params; 
    auto node = std::make_shared<rclcpp::Node>("fibonacci_action_client");
    params.nh = node;
    params.default_port_value = "fibonacci";
    factory.registerNodeType<FibonacciAction>("Fibonacci", params);
    factory.registerNodeType<GetBBvalueNode<vec_int>>("GetVec", get_vec, on_err);

    
    auto server_node = std::make_shared<FibonacciActionServer>();
    auto spin_callback = [](rclcpp::Node::SharedPtr node){rclcpp::spin(node);};
    std::thread{spin_callback, server_node}.detach();
    sleep();

    // tick the tree
    auto tree = factory.createTreeFromText(xml_text);
    tree.tickWhileRunning();

    rclcpp::shutdown();

    return 0;
}

The code is essentially copied-pasted from the tutorials BT ros2 integration and ROS2 action server.

When I try to run the executable I get the following:

root@f1e379732b0f:~/projects# ros2 run bt_act_test bt_act_test 
[INFO] [1687792324.400457200] [server_node]: Received goal request with order 5
[INFO] [1687792324.401037500] [server_node]: Executing goal
[INFO] [1687792324.401250200] [server_node]: Publish feedback
[INFO] [1687792324.421203700] [fibonacci_action_client]: Goal accepted by server, waiting for result
[INFO] [1687792325.401689100] [server_node]: Publish feedback
[INFO] [1687792325.407532400] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 
0; 1; 1; 2; 
[INFO] [1687792326.401572200] [server_node]: Publish feedback
[INFO] [1687792326.405681700] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 
0; 1; 1; 2; 3; 
[INFO] [1687792326.406163200] [server_node]: Received request to cancel goal
terminate called after throwing an instance of 'std::runtime_error'
  what():  Asked to publish result for goal that does not exist
[ros2run]: Aborted

Apologies for the long message, but I hope it will make the error reproduction easier.
Thanks in advance for the support

[Stewil]

I am having more or less the same issue. Our structure is suprisingly similar. However i am getting a different error message

[INFO] [1687943587.313014132] [action_test_client_1]: Goal handle: goal is 2
terminate called after throwing an instance of 'rclcpp_action::exceptions::UnknownGoalHandleError'
  what():  Goal handle is not known to this client

i do not get the Received request to cancel goal even though i have the same setup.

I noticed the error does not happen with a regular Sequence instead of a Reactive Sequence, however is not my intended behaviour.

this is my tree:

<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
  <BehaviorTree ID="MainLogic">
    <ReactiveSequence>
      <Fallback>
        <GET_ROV_STATUS/>
        <ACTION_TEST_CLIENT_2 goal="1"/>
      </Fallback>
      <ACTION_TEST_CLIENT_1 goal="2"/>
    </ReactiveSequence>
  </BehaviorTree>

  <!-- Description of Node Models (used by Groot) -->
  <TreeNodesModel>
    <Action ID="ACTION_TEST_CLIENT_1"
            editable="true">
      <input_port name="goal"/>
    </Action>
    <Action ID="ACTION_TEST_CLIENT_2"
            editable="true">
      <input_port name="goal"/>
    </Action>
    <Action ID="GET_ROV_STATUS"
            editable="true"/>
  </TreeNodesModel>

</root>

adding my gdb backtrace in hopes of this helping:

#0  __pthread_kill_implementation (no_tid=0, signo=6, threadid=140737339827200)
    at ./nptl/pthread_kill.c:44
#1  __pthread_kill_internal (signo=6, threadid=140737339827200) at ./nptl/pthread_kill.c:78
#2  __GI___pthread_kill (threadid=140737339827200, signo=signo@entry=6)
    at ./nptl/pthread_kill.c:89
#3  0x00007ffff74f3476 in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff74d97f3 in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff779dbbe in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff77a924c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff77a92b7 in std::terminate() () from /lib/x86_64-linux-gnu/libstdc++.so.6
#8  0x00007ffff77a9518 in __cxa_throw () from /lib/x86_64-linux-gnu/libstdc++.so.6
#9  0x0000555555598600 in rclcpp_action::Client<example_interfaces::action::Fibonacci>::async_cancel_goal(std::shared_ptr<rclcpp_action::ClientGoalHandle<example_interfaces::action::Fibonacci> >, std::function<void (std::shared_ptr<action_msgs::srv::CancelGoal_Response_<std::allocator<void> > >)>) (this=0x555555ba6460, 
    goal_handle=std::shared_ptr<rclcpp_action::ClientGoalHandle<example_interfaces::action::Fibonacci>> (empty) = {...}, cancel_callback=...)
    at /opt/ros/humble/include/rclcpp_action/rclcpp_action/client.hpp:536
#10 0x00005555555951a0 in BT::RosActionNode<example_interfaces::action::Fibonacci>::cancelGoal (this=0x555555b85c20)
    at /---/---/ros2ws/install/behaviortree_ros2/include/behaviortree_ros2/behaviortree_ros2/bt_action_node.hpp:408
#11 0x0000555555591a44 in BT::RosActionNode<example_interfaces::action::Fibonacci>::halt (
    this=0x555555b85c20)
--Type <RET> for more, q to quit, c to continue without paging--
    at /---/---/ros2ws/install/behaviortree_ros2/include/behaviortree_ros2/behaviortree_ros2/bt_action_node.hpp:401
#12 0x00007ffff7c9f2c3 in BT::TreeNode::haltNode() ()
   from /usr/local/lib/libbehaviortree_cpp.so
#13 0x00007ffff7c9d7a8 in BT::ControlNode::haltChild(unsigned long) ()
   from /usr/local/lib/libbehaviortree_cpp.so
#14 0x00007ffff7ce0ecb in BT::ReactiveSequence::tick() ()
   from /usr/local/lib/libbehaviortree_cpp.so
#15 0x00007ffff7ca099d in BT::TreeNode::executeTick() ()
   from /usr/local/lib/libbehaviortree_cpp.so
#16 0x00007ffff7c6cc0e in BT::Tree::tickRoot(BT::Tree::TickOption, std::chrono::duration<long, std::ratio<1l, 1000l> >) () from /usr/local/lib/libbehaviortree_cpp.so
#17 0x0000555555566f21 in main (argc=1, argv=0x7fffffffd888)
    at /---/---/ros2ws/src/shared_playground/src/behavior_tree.cpp:40

i hope we can find out what's causing this. To me it seems like the action stems from the action server goal cancel itself, but i do not understand why it works with a regular Sequence then.

[facontidavide]

I refactored the code.
I will try your example soon

[ErickKramer]

Hi,
I somehow landed into the same runtime error

I am not really sure why one of our Custom BT Nodes based on the bt_action_node is falling under this behavior. As far as I have seen, there is nothing out of the ordinary with it.

Anyways, I opted to handle this behavior by catching the exception that is thrown by the action_client_->async_get_result and action_client_->async_cancel_goal as done in https://github.com/ErickKramer/BehaviorTree.ROS2/blob/c125b15e5dee9b697bcdf7c1c68213f159b9a9f0/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp#L439

I'd be glad to open PR if you find such a solution good enough.

[ErickKramer]

Hi, I somehow landed into the same runtime error

I am not really sure why one of our Custom BT Nodes based on the bt_action_node is falling under this behavior. As far as I have seen, there is nothing out of the ordinary with it.

Anyways, I opted to handle this behavior by catching the exception that is thrown by the action_client_->async_get_result and action_client_->async_cancel_goal as done in https://github.com/ErickKramer/BehaviorTree.ROS2/blob/c125b15e5dee9b697bcdf7c1c68213f159b9a9f0/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp#L439

I'd be glad to open PR if you find such a solution good enough.

Further insight. This seems to happen when setGoal of the Action Node hasn't finished by the time the halt is triggered.

[marj3220]

I'm getting the exact same error as @ErickKramer.

[facontidavide]

ok, I will investigate

[danielmohansahu]

I ran into the same issue as well. It appears to be a race condition between server-side goal acceptance / rejection and client side cancellation.

I "solved" it by adding the following to the top of RosActionNode<T>::cancelGoal():

template<class T> inline
  void RosActionNode<T>::cancelGoal()
{
  // block until we get the goal handle (need it to cancel)
  if( !goal_received_ )
  {
    // process callbacks until future returns (could be indefinite...)
    callback_group_executor_.spin_until_future_complete(future_goal_handle_);

    // update variables
    goal_received_ = true;
    goal_handle_ = future_goal_handle_.get();
    future_goal_handle_ = {};

    // if no goal handle the goal was rejected; easy exit
    if( !goal_handle_ )
    {
      return;
    }
  }
  // ... rest of function unchanged

Unfortunately this won't work in the general case since it introduces a potentially indefinite blocking call into cancelGoal... not sure what the right solution looks like.

[leeminju531]

I faced similar issues in case of inherited RosActionNode. [in commit : bdb778a]

In my case, most issues occurs due to not catching up GoalHandler in RosActionNode.
It appears that there is a need for a term to be invoked or scheduled in an executor.

In RosActionNode, implemented like this :

      auto nodelay = std::chrono::milliseconds(0);
      auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000);
      auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay);

That executor tries to find out goal_handler asynchronously, but spin_until_future_complete seems to check out only one execution at once, while other executions are also present in it. So it appears to need a term to be invoked.

In my case, replacing it with callback_group_executor_.spin_until_future_complete(future_goal_handle_, std::chrono::milliseconds(30)) works well.

[LihanChen2004]

Same question here.

BehaviorTree.CPP: release 4.6.2
BehaviorTree.ROS2: commit cc31ea7

When I use the SendNav2Goal Action separately in the main tree and the subtree, if the Action in the subtree cancels the goal, the next SendNav2Goal Action in the main tree keeps getting stuck in the Running state. Additionally, when I shutdown the program using Ctrl+C, I get the following error:

[pb2025_sentry_behavior-1] [INFO] [1738331516.197563673] [red_standard_robot1.pb2025_sentry_behavior]: Distance remaining: 0.145711
[pb2025_sentry_behavior-1] [INFO] [1738331516.208662795] [red_standard_robot1.pb2025_sentry_behavior]: Navigation succeeded!
[pb2025_sentry_behavior-1] [INFO] [1738331523.459382232] [bt_action_server]: Received request to cancel goal
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[pb2025_sentry_behavior-1] [INFO] [1738331525.782581212] [rclcpp]: signal_handler(signum=2)
[pb2025_sentry_behavior-1] terminate called after throwing an instance of 'rclcpp_action::exceptions::UnknownGoalHandleError'
[pb2025_sentry_behavior-1]   what():  Goal handle is not known to this client.
[ERROR] [pb2025_sentry_behavior-1]: process has died [pid 440737, exit code -6, cmd '/home/lihanchen/DECISION_WS/pb20205_sentry_decision_ws/install/pb2025_sentry_behavior/lib/pb2025_sentry_behavior/pb2025_sentry_behavior --ros-args --log-level info --ros-args -r __node:=pb2025_sentry_behavior -r __ns:=/red_standard_robot1 --params-file /tmp/launch_params_argcr6ce -r /tf:=tf -r /tf_static:=tf_static'].

If the subtree does not trigger Action cancelGoal, then the shutdown does not produce the terminate called after throwing an instance of 'rclcpp_action::exceptions::UnknownGoalHandleError' error.

For example, in the video below:

Expected behavior: When robot_hp > 399, the robot goes to the supply point (0,0) and, once it arrives, immediately moves to the (2,0) point, repeatedly moving back and forth. If robot_hp < 399, the robot waits at the supply point (0,0) until robot_hp > 399, after which it proceeds to the (2,0) point.

Actual behavior: The robot waits at the supply point (0,0) until robot_hp > 399. However, after reaching the (2,0) point, the behavior tree gets stuck. It shows that the action of going to the (2,0) point is still in the RUNNING state, but in reality, nav2 has already sent a "reached" feedback.

SendNav2Goal Action plugin:

send_nav2_goal.hpp

#include <memory>
#include <string>

#include "behaviortree_ros2/bt_action_node.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"

namespace pb2025_sentry_behavior
{
class SendNav2GoalAction : public BT::RosActionNode<nav2_msgs::action::NavigateToPose>
{
public:
  SendNav2GoalAction(
    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params);

  static BT::PortsList providedPorts();

  bool setGoal(Goal & goal) override;

  void onHalt() override;

  BT::NodeStatus onResultReceived(const WrappedResult & wr) override;

  BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) override;

  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override;
};
}  // namespace pb2025_sentry_behavior

#endif  // PB2025_SENTRY_BEHAVIOR__PLUGINS__ACTION__SEND_NAV2_GOAL_HPP_

send_nav2_goal.cpp

#include "pb2025_sentry_behavior/plugins/action/send_nav2_goal.hpp"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

namespace pb2025_sentry_behavior
{

SendNav2GoalAction::SendNav2GoalAction(
  const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: RosActionNode<nav2_msgs::action::NavigateToPose>(name, conf, params)
{
}

bool SendNav2GoalAction::setGoal(nav2_msgs::action::NavigateToPose::Goal & goal)
{
  auto goal_x = getInput<float>("goal_x");
  auto goal_y = getInput<float>("goal_y");
  auto goal_yaw = getInput<float>("goal_yaw");
  tf2::Quaternion quaternion;
  quaternion.setRPY(0, 0, goal_yaw.value());

  goal.pose.header.frame_id = "map";
  goal.pose.header.stamp = now();
  goal.pose.pose.position.x = goal_x.value();
  goal.pose.pose.position.y = goal_y.value();
  goal.pose.pose.orientation = tf2::toMsg(quaternion);

  RCLCPP_DEBUG(
    logger(), "Setting goal to (%.2f, %.2f, %.2f)", goal_x.value(), goal_y.value(),
    goal_yaw.value());

  return true;
}

BT::NodeStatus SendNav2GoalAction::onResultReceived(const WrappedResult & wr)
{
  switch (wr.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(logger(), "Navigation succeeded!");
      return BT::NodeStatus::SUCCESS;

    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(logger(), "Navigation aborted by server");
      return BT::NodeStatus::FAILURE;

    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_WARN(logger(), "Navigation canceled");
      return BT::NodeStatus::FAILURE;

    default:
      RCLCPP_ERROR(logger(), "Unknown navigation result code: %d", static_cast<int>(wr.code));
      return BT::NodeStatus::FAILURE;
  }
}

BT::NodeStatus SendNav2GoalAction::onFeedback(
  const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback)
{
  RCLCPP_INFO(logger(), "Distance remaining: %f", feedback->distance_remaining);
  return BT::NodeStatus::RUNNING;
}

void SendNav2GoalAction::onHalt() { RCLCPP_INFO(logger(), "SendNav2GoalAction has been halted."); }

BT::NodeStatus SendNav2GoalAction::onFailure(BT::ActionNodeErrorCode error)
{
  RCLCPP_ERROR(logger(), "SendNav2GoalAction failed with error code: %d", error);
  return BT::NodeStatus::FAILURE;
}

BT::PortsList SendNav2GoalAction::providedPorts()
{
  BT::PortsList additional_ports = {
    BT::InputPort<float>("goal_x", 0.0, "Goal x coordinate"),
    BT::InputPort<float>("goal_y", 0.0, "Goal y coordinate"),
    BT::InputPort<float>("goal_yaw", 0.0, "Goal orientation (yaw)"),
  };
  return providedBasicPorts(additional_ports);
}

}  // namespace pb2025_sentry_behavior

#include "behaviortree_ros2/plugins.hpp"
CreateRosNodePlugin(pb2025_sentry_behavior::SendNav2GoalAction, "SendNav2Goal");

2025.1.31behavior_tree_action_bug.mp4

Edited: Same result if not using subtree.

[tropappar]

Problem persists. Solution by @ErickKramer works for me: 2fdf4a5