#include "my_behavior_tree/ok_status.hpp"

using namespace BT;

OKStatus::OKStatus(const std::string& name,
                            const NodeConfig& conf,
                            const RosNodeParams& params)
: RosTopicSubNode<std_msgs::msg::String>(name, conf, params)
{

}

BT::PortsList OKStatus::providedPorts()
{
    return{InputPort<std::string>("topic_name")};
}

// Check if status is okay or not, if not return failure. Used to control state of the tree
NodeStatus OKStatus::onTick(const std::shared_ptr<std_msgs::msg::String>& last_msg)
  {    // TODO - Add multiple state checks

    if (last_msg)
    {
      status = last_msg->data;

      if(status =="OK") 
    {
      // RCLCPP_INFO(logger(), "Not in error"); 
      c = 0;
      return NodeStatus::SUCCESS; 
    }
    else
    {
      c =1;
      // RCLCPP_INFO(logger(), "In error");
      return NodeStatus::FAILURE; 
    }

    }

    else
    {
      
      if(c ==0) 
    {
      return NodeStatus::SUCCESS; 
    }
    else
    {
        return NodeStatus::FAILURE; 
    }
       

    } 
    
    
  }

#include "behaviortree_ros2/bt_topic_sub_node.hpp"
#include <std_msgs/msg/string.hpp>

using namespace BT;

class OKStatus: public RosTopicSubNode<std_msgs::msg::String>
{
public:
  OKStatus(const std::string& name,
                const NodeConfig& conf,
                const RosNodeParams& params);

  static BT::PortsList providedPorts();

  NodeStatus onTick(const std::shared_ptr<std_msgs::msg::String>& last_msg) override;

  std::string status;
  int c;
};

#include <gtest/gtest.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <ament_index_cpp/get_package_prefix.hpp>
#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <string>
#include <fstream>
#include <memory>
#include <utility>
#include <boost/filesystem.hpp>
#include <my_behavior_tree/ok_status.hpp>

using namespace BT;


std_msgs::msg::String createString(const std::string& name)
{
    std_msgs::msg::String sample;
    sample.data = name;
    return sample;
}

class BehaviorTreeTestFixture : public ::testing::Test
{

protected:
    std::shared_ptr<BehaviorTreeHandler> bt_handler;
    std_msgs::msg::String a;

public:
    void SetUp() override
    {
        bt_handler = std::make_shared<BehaviorTreeHandler>();
        a = createString("OK");
        
    }

    void TearDown() override
    {
        bt_handler.reset();
    }

};

TEST_F(BehaviorTreeTestFixture, TestSuccess)
{

rclcpp::init(0, nullptr);


auto nh = std::make_shared<rclcpp::Node>("tree_test_node");

RosNodeParams params;
params.nh = nh;
params.default_port_value = "test";

factory.registerNodeType<OKStatus>("OKStatus", params);

OKStatus ok_status("ok_status",NodeConfig(),RosNodeParams());

auto message_ptr = std::make_shared<std_msgs::msg::String>(a);

auto status = ok_status.onTick(message_ptr);
EXPECT_EQ(status, BT::NodeStatus::SUCCESS);

rclcpp::shutdown();

             <Sequence>
                <SubTree ID="DisableStopOnObstacle" _autoremap="true"/>
                <ReactiveSequence>
                    <Sequence>
                        <SubTree ID="CheckEStop" _autoremap="true"/>
                        <SubTree ID="CheckContinueMissionDocking" _autoremap="true"/>
                    </Sequence>
                    <DockingAction
                        action_name="/docking_action_server"
                        distance="-0.1"
                        linear_speed="0.03"
                        stop_at_charger="false"
                        stop_at_mag_marker="false"
                        target_id="{docking_id}"
                        allowed_time="30"
                        failure_if_no_success = "true"
                        level="{level}"
                        message="{message}"
                    />
                </ReactiveSequence>
            </Sequence>

[ERROR] [1715003079.255998664] [bt_action_server]: Failed to load BehaviorTree: tree.xml 
 Error at line 7: -> Node not recognized: MyNode

<root BTCPP_format="4" main_tree_to_execute="MainTree">
    <BehaviorTree ID="test_tree">
        <GetDigitalSignal topic_name="{test_topic_name}" value="{output}"/>
    </BehaviorTree>

    <BehaviorTree ID="MainTree">
        <Sequence>
	    <Script code="my_topic:='test_topic'"/>
            <Delay delay_msec="10000"> <!-- Added Delay to allow some time connecting to Groot-->
                <KeepRunningUntilFailure>
                    <ForceSuccess>
                        <Delay delay_msec="3000"> <!-- Delay added to see in Groot what happens easier-->
                            <ParallelAll>
                                <GetDigitalSignal topic_name="test_topic" value="{my_output_static}"/>
                                <GetDigitalSignal topic_name="{my_topic}" value="{my_output_scripted}"/>
                                <SubTree ID="test_tree" test_topic_name="{my_topic}" output="{my_ouput_subtree}"/>
                            </ParallelAll>
                        </Delay>
                    </ForceSuccess>
                </KeepRunningUntilFailure>
            </Delay>
        </Sequence>
    </BehaviorTree>
</root>

static BT::PortsList providedPorts()
{
    return providedBasicPorts({
        BT::OutputPort<std_msgs::msg::Bool::_data_type>("value", "Current value of the digital signal"),
    });
}

BT::NodeStatus onTick(const std_msgs::msg::Bool::SharedPtr &msg)
{
    if (msg != nullptr)
    {
        setOutput("value", msg->data);
        return BT::NodeStatus::SUCCESS;
    }

    return BT::NodeStatus::FAILURE;
}

[ERROR] [1715013001.175801122] [bt_action_server]: Behavior Tree exception:bad_weak_ptr

[ERROR] [1715013002.693655469] [bt_action_server]: Behavior Tree exception:The ROS node went out of scope. RosNodeParams doesn't take the ownership of the node.

add_library(dummy_sleep_action_bt_node SHARED plugins/action/DummySleepAction.cpp)
list(APPEND plugin_libs dummy_sleep_action_bt_node)

foreach(bt_plugin ${plugin_libs})
  ament_target_dependencies(${bt_plugin} ${dependencies})
  target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
endforeach()

// in main()
  BehaviorTreeFactory factory;

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

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()

...
    <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>
...

#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;
}

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

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'Service type support not from this implementation. Got:
    Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:111
    Could not load library libbehaviortree_ros2__rosidl_typesupport_introspection_cpp.so: dlopen error: libbehaviortree_ros2__rosidl_typesupport_introspection_cpp.so: cannot open shared object file: No such file or directory, at ./src/shared_library.c:99, at ./src/type_support_dispatch.hpp:76
while fetching it, at ./src/rmw_node.cpp:4503'

with this new error message:

  'type_support is null, at ./src/rmw_node.cpp:4573'

rcutils_reset_error() should be called after error handling to avoid this.
<<<
terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
  what():  could not initialize rcl action client: type_support is null, at ./src/rmw_node.cpp:4573, at ./src/rcl/client.c:113
[1]    149016 IOT instruction (core dumped)  ./install/behaviortree_ros2/bin/sleep_client

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'Service type support not from this implementation. Got:
    Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:111
    Could not load library libbehaviortree_ros2__rosidl_typesupport_introspection_cpp.so: dlopen error: libbehaviortree_ros2__rosidl_typesupport_introspection_cpp.so: cannot open shared object file: No such file or directory, at ./src/shared_library.c:99, at ./src/type_support_dispatch.hpp:76
while fetching it, at ./src/rmw_node.cpp:4503'

with this new error message:

  'type_support is null, at ./src/rmw_node.cpp:4573'

rcutils_reset_error() should be called after error handling to avoid this.
<<<
terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
  what():  type_support is null, at ./src/rmw_node.cpp:4573, at ./src/rcl/service.c:124
[1]    149048 IOT instruction (core dumped)  ./install/behaviortree_ros2/bin/sleep_server

PrintValue: start
[INFO] [1695293776.715275411] [sleep_client]: RESULT_GOAL ID : ef75e3cbdd7c226bb1c6e2cd36a0c236
[INFO] [1695293776.715445058] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695293778.268966187] [sleep_client]: CANCEL_GOAL ID : dd28f1f6ab96854d84f85a2fdde2a2
[INFO] [1695293778.269641275] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695293780.270740751] [sleep_client]: RESULT_GOAL ID : 1dcb1f40d8421bd4f824346bfb878b
[INFO] [1695293780.270829274] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695293780.271580174] [sleep_client]: RESULT_GOAL ID : dd28f1f6ab96854d84f85a2fdde2a2
[INFO] [1695293781.773432366] [sleep_client]: CANCEL_GOAL ID : 2160659ffbfa6596de53aabe02fa9e5
[INFO] [1695293781.774070320] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695293783.775155203] [sleep_client]: RESULT_GOAL ID : 956327c886842cca4da9a83ba9a375df
[INFO] [1695293783.775239360] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695293783.775899932] [sleep_client]: RESULT_GOAL ID : 2160659ffbfa6596de53aabe02fa9e5
[INFO] [1695293785.275925308] [sleep_client]: CANCEL_GOAL ID : 45126ab2219b8e3861537d843c991
[INFO] [1695293785.276538092] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695293787.277526354] [sleep_client]: RESULT_GOAL ID : dd451ef6592bf2b73c6935dc4cf70d6
[INFO] [1695293787.277612585] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695293787.278468184] [sleep_client]: RESULT_GOAL ID : 45126ab2219b8e3861537d843c991
[INFO] [1695293788.783090360] [sleep_client]: CANCEL_GOAL ID : acbe325acc152b348c40c6a96178fd8c
[INFO] [1695293788.783685525] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695293790.784665696] [sleep_client]: RESULT_GOAL ID : 3a38ae54fb365a81490d7737b49a8
[INFO] [1695293790.784733127] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695293790.785492102] [sleep_client]: RESULT_GOAL ID : acbe325acc152b348c40c6a96178fd8c
[INFO] [1695293792.286093402] [sleep_client]: CANCEL_GOAL ID : 8688086cee0346310fbdc595213e
[INFO] [1695293792.286707982] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted

PrintValue: start
[INFO] [1695295344.142244774] [sleep_client]: RESULT_GOAL ID : f9f6d789aa5114b51857d44126bc662
[INFO] [1695295344.142411571] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695295345.643276985] [sleep_client]: CANCEL_GOAL ID : e5261931333755dc253252dfb2e861d9
[INFO] [1695295345.743568867] [sleep_client]: RESULT_GOAL ID : e5261931333755dc253252dfb2e861d9
[INFO] [1695295345.743714297] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695295347.744981916] [sleep_client]: RESULT_GOAL ID : c2baf96cb5b79a7674a94352a087383f
[INFO] [1695295347.745074847] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695295349.264379159] [sleep_client]: CANCEL_GOAL ID : 5454cc3c188455c138a998b5d9280
[INFO] [1695295349.346670567] [sleep_client]: RESULT_GOAL ID : 5454cc3c188455c138a998b5d9280
[INFO] [1695295349.346815698] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695295351.348284469] [sleep_client]: RESULT_GOAL ID : 1ab7cf57e54120eaf0fceeddd65562c4
[INFO] [1695295351.348366768] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695295352.868317555] [sleep_client]: CANCEL_GOAL ID : f276a469da4f1365db2cd3bc79961469
[INFO] [1695295352.949544723] [sleep_client]: RESULT_GOAL ID : f276a469da4f1365db2cd3bc79961469
[INFO] [1695295352.949629849] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695295354.950950643] [sleep_client]: RESULT_GOAL ID : e99b9b5b9c75a854f22234197362ff9
[INFO] [1695295354.951044802] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695295356.471232107] [sleep_client]: CANCEL_GOAL ID : 69b57badefdb26a65bafeaca7b2a4537
[INFO] [1695295356.552313938] [sleep_client]: RESULT_GOAL ID : 69b57badefdb26a65bafeaca7b2a4537
[INFO] [1695295356.552409254] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted
PrintValue: start
[INFO] [1695295358.553869406] [sleep_client]: RESULT_GOAL ID : eee66aede6adfc71a2326b49d98fd677
[INFO] [1695295358.553942439] [sleep_client]: sleepA: onResultReceived. Done = true
PrintValue: sleep completed
[INFO] [1695295360.058143973] [sleep_client]: CANCEL_GOAL ID : 6cafc4d991f0ff796b338252e123618
[INFO] [1695295360.155102960] [sleep_client]: RESULT_GOAL ID : 6cafc4d991f0ff796b338252e123618
[INFO] [1695295360.155213032] [sleep_client]: sleepB: onHalt
PrintValue: sleep aborted

RUN mkdir -p $HOME/$EBOTS_ARMS_WORKSPACE/behaviortree_cpp && \
    git clone \
    https://github.com/BehaviorTree/BehaviorTree.CPP.git \
    $HOME/$EBOTS_ARMS_WORKSPACE/behaviortree_cpp && \
    cd $HOME/$EBOTS_ARMS_WORKSPACE/behaviortree_cpp && \
    mkdir build && \
    cd build && \
    cmake .. && \
    make && \
    make install

RUN mkdir -p $HOME/$EBOTS_ARMS_WORKSPACE/behaviortree_ros && \
    git clone \
    https://github.com/BehaviorTree/BehaviorTree.ROS2.git \
    $HOME/$EBOTS_ARMS_WORKSPACE/behaviortree_ros

# I don't think the submodules are necessary if we do the rosdep install? *unsure*
RUN mkdir -p $HOME/$EBOTS_ARMS_WORKSPACE/groot && \
    git clone \
    https://github.com/BehaviorTree/Groot \
    $HOME/$EBOTS_ARMS_WORKSPACE/groot && \
    cd $HOME/$EBOTS_ARMS_WORKSPACE/groot && \
    git submodule update --init --recursive && \
    mkdir build && \
    cd build && \
    cmake .. && \
    make

# Install dependencies of all packages
RUN cd $HOME/$EBOTS_ARMS_WORKSPACE && \
    rosdep install -r \
    --from-paths . -y

--- stderr: behaviortree_ros2
In file included from /home/ebots/workspace/behaviortree_ros/test/test_client.cpp:1:
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp: In lambda function:
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:217:7: error: there are no arguments to ‘emitStateChanged’ that depend on a template parameter, so a declaration of ‘emitStateChanged’ must be available [-fpermissive]
  217 |       emitStateChanged();
      |       ^~~~~~~~~~~~~~~~
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:217:7: note: (if you use ‘-fpermissive’, G++ will accept your code, but allowing the use of an undeclared name is deprecated)
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp: In lambda function:
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:225:7: error: there are no arguments to ‘emitStateChanged’ that depend on a template parameter, so a declaration of ‘emitStateChanged’ must be available [-fpermissive]
  225 |       emitStateChanged();
      |       ^~~~~~~~~~~~~~~~
/home/ebots/workspace/behaviortree_ros/test/test_client.cpp: In function ‘int main(int, char**)’:
/home/ebots/workspace/behaviortree_ros/test/test_client.cpp:112:10: error: ‘class BT::Tree’ has no member named ‘sleep’
  112 |     tree.sleep(std::chrono::milliseconds(100));
      |          ^~~~~
In file included from /home/ebots/workspace/behaviortree_ros/test/test_client.cpp:1:
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp: In instantiation of ‘BT::NodeStatus BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]’:
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:177:14:   required from here
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:217:23: error: ‘emitStateChanged’ was not declared in this scope
  217 |       emitStateChanged();
      |       ~~~~~~~~~~~~~~~~^~
/home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:225:23: error: ‘emitStateChanged’ was not declared in this scope
  225 |       emitStateChanged();
      |       ~~~~~~~~~~~~~~~~^~
In file included from /usr/include/c++/9/future:48,
                 from /opt/ros/foxy/include/rclcpp/executors.hpp:18,
                 from /home/ebots/workspace/behaviortree_ros/include/behaviortree_ros2/bt_action_node.hpp:6,
                 from /home/ebots/workspace/behaviortree_ros/test/test_client.cpp:1:
/usr/include/c++/9/bits/std_function.h: At global scope:
/usr/include/c++/9/bits/std_function.h:523:2: error: ‘std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> std::function<_Res(_ArgTypes ...)>::operator=(_Functor&&) [with _Functor = BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]::<lambda(rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep>::SharedPtr, std::shared_ptr<const behaviortree_ros2::action::Sleep_Feedback_<std::allocator<void> > >)>; _Res = void; _ArgTypes = {std::shared_ptr<rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep> >, std::shared_ptr<const behaviortree_ros2::action::Sleep_Feedback_<std::allocator<void> > >}; std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> = std::function<void(std::shared_ptr<rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep> >, std::shared_ptr<const behaviortree_ros2::action::Sleep_Feedback_<std::allocator<void> > >)>&]’, declared using local type ‘BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]::<lambda(rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep>::SharedPtr, std::shared_ptr<const behaviortree_ros2::action::Sleep_Feedback_<std::allocator<void> > >)>’, is used but never defined [-fpermissive]
  523 |  operator=(_Functor&& __f)
      |  ^~~~~~~~
/usr/include/c++/9/bits/std_function.h:523:2: error: ‘std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> std::function<_Res(_ArgTypes ...)>::operator=(_Functor&&) [with _Functor = BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]::<lambda(const WrappedResult&)>; _Res = void; _ArgTypes = {const rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep>::WrappedResult&}; std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> = std::function<void(const rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep>::WrappedResult&)>&]’, declared using local type ‘BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]::<lambda(const WrappedResult&)>’, is used but never defined [-fpermissive]
/usr/include/c++/9/bits/std_function.h:523:2: error: ‘std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> std::function<_Res(_ArgTypes ...)>::operator=(_Functor&&) [with _Functor = BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]::<lambda(std::shared_future<std::shared_ptr<rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep> > >)>; _Res = void; _ArgTypes = {std::shared_future<std::shared_ptr<rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep> > >}; std::function<_Res(_ArgTypes ...)>::_Requires<std::function<_Res(_ArgTypes ...)>::_Callable<typename std::decay<_Functor>::type>, std::function<_Res(_ArgTypes ...)>&> = std::function<void(std::shared_future<std::shared_ptr<rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep> > >)>&]’, declared using local type ‘BT::RosActionNode<ActionT>::tick() [with ActionT = behaviortree_ros2::action::Sleep]::<lambda(std::shared_future<std::shared_ptr<rclcpp_action::ClientGoalHandle<behaviortree_ros2::action::Sleep> > >)>’, is used but never defined [-fpermissive]
make[2]: *** [CMakeFiles/test_client.dir/build.make:63: CMakeFiles/test_client.dir/test/test_client.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:126: CMakeFiles/test_client.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< behaviortree_ros2 [31.0s, exited with code 2]
Aborted  <<< behaviortree_cpp [1min 50s]

Summary: 5 packages finished [1min 51s]
  1 package failed: behaviortree_ros2
  6 packages aborted: behaviortree_cpp control_msgs controller_manager_msgs groot industrial_msgs rviz_visual_tools
  2 packages had stderr output: behaviortree_cpp behaviortree_ros2
  10 packages not processed
The command '/bin/sh -c cd $HOME/$EBOTS_ARMS_WORKSPACE &&     . /opt/ros/$ROS_DISTRO/setup.sh &&     colcon build --allow-overriding control_msgs      controller_interface hardware_interface     joint_state_publisher joint_state_publisher_gui     controller_manager ros2_control_test_assets --cmake-args -DCMAKE_BUILD_TYPE=Release' returned a non-zero code: 2

<root BTCPP_format="4">
    <BehaviorTree>
        <Sequence>
            <PrintValue message="start sleepAction"/>
            <SleepAction name="sleepA" msec="5000"/>
            <PrintValue message="sleepAction completed"/>
            <PrintValue message="start sleepNormal"/>
            <Sleep msec="6000"/>
            <PrintValue message="sleepNormal completed"/>
        </Sequence>
    </BehaviorTree>
</root>

<root BTCPP_format="4">
    <BehaviorTree>
        <Sequence>
            <PrintValue message="start sleepAction"/>
            <SleepAction name="sleepA" msec="5000"/>
            <PrintValue message="sleepAction completed"/>
        </Sequence>
    </BehaviorTree>
</root>

<root BTCPP_format="4">
    <BehaviorTree>
        <Sequence>
            <PrintValue message="start sleepNormal"/>
            <Sleep msec="6000"/>
            <PrintValue message="sleepNormal completed"/>
        </Sequence>
    </BehaviorTree>
</root>

$ ros2 launch btcpp_ros2_samples sample_bt_executor.launch.xml
[INFO] [launch]: All log files can be found below /home/ros/.ros/log/2024-05-15-11-04-28-365715-ros-desktop-72895
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [sample_bt_executor-1]: process started with pid [72896]
[INFO] [sleep_server-2]: process started with pid [72898]
[sample_bt_executor-1] [INFO] [1715796268.500210508] [bt_action_server]: Starting Action Server: behavior_server
[sample_bt_executor-1] terminate called after throwing an instance of 'std::filesystem::__cxx11::filesystem_error'
[sample_bt_executor-1]   what():  filesystem error: directory iterator cannot open directory: No such file or directory [/opt/ros/humble/share/behaviortree_cpp/bt_plugins]
[ERROR] [sample_bt_executor-1]: process has died [pid 72896, exit code -6, cmd '/home/ros/prebuilt_ws/install/btcpp_ros2_samples/lib/btcpp_ros2_samples/sample_bt_executor --ros-args --params-file /home/ros/prebuilt_ws/install/btcpp_ros2_samples/share/btcpp_ros2_samples/config/sample_bt_executor.yaml'].
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[sleep_server-2] [INFO] [1715796270.557774044] [rclcpp]: signal_handler(signum=2)
[INFO] [sleep_server-2]: process has finished cleanly [pid 72898]

#include "segmentation_bt_client.hpp"
#include "move_bt_action.hpp"

#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/loggers/bt_cout_logger.h"
#include "behaviortree_ros2/plugins.hpp"

using namespace BT;


int main(int argc, char** argv)
{

    const std::string bt_xml_path = "/home/diruscio/ros2_proj/src/parcel_pkg/config/bt_xml.xml";
    rclcpp::init(argc,argv);

    auto node = std::make_shared<rclcpp::Node>("bt_main");
    node->declare_parameter("bt_xml", rclcpp::ParameterValue(bt_xml_path));
    std::string bt_xml;
    node->get_parameter("bt_xml", bt_xml);

    
    RosNodeParams params_action; 
    params_action.nh = node;
    params_action.default_port_value="move_action_server";


    RosNodeParams params_service;
    params_service.nh = node;
    params_service.default_port_value = "cloud_segmentation_server";

    BehaviorTreeFactory factory;
    factory.registerNodeType<MoveToTargetAction>("MoveToTarget", params_action);    
    factory.registerNodeType<SegmentationClient>("SegmentationClient", params_service);
    
    auto tree = factory.createTreeFromFile(bt_xml);
    tree.tickWhileRunning();
    std::this_thread::sleep_for(std::chrono::milliseconds(10000));

    return 0;
}

<root BTCPP_format="4" >
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <MoveToTarget     move_type="home" target_frame="{target_frame}" />
            <SegmentationClient target_frame="{target_frame}"/>
            <Repeat num_cycles="3">
                <Sequence>
                    <MoveToTarget     move_type="pick"    target_frame="{target_frame}"/>
                    <MoveToTarget     move_type="away"    target_frame="{target_frame}"/>
                    <MoveToTarget     move_type="release"    target_frame="{target_frame}"/>            
                    <SegmentationClient target_frame="{target_frame}"/>
                </Sequence>
            </Repeat>
        </Sequence>
    </BehaviorTree>
</root>

<root BTCPP_format="4">
  <BehaviorTree ID="tree">

    <Sequence>
      <TickRemoteTree robot_name="A" var="1"/>
      <TickRemoteTree robot_name="B" var="2"/>

      <TickRemoteTree robot_name="A" var="10"/>
      <TickRemoteTree robot_name="B" var="20"/> 
    
      <TickRemoteTree robot_name="A" var="100"/>
      <TickRemoteTree robot_name="B" var="200"/>
    </Sequence>

  </BehaviorTree>
</root>

$ sleep_server
[rcutils|error_handling.c:66] an error string (message, file name, or formatted message) will 
be truncated

future_response_ = service_client_->async_send_request(request).share();

#pragma once

#include <behaviortree_ros2/bt_action_node.hpp>
#include <iostream>

#include "behaviortree_cpp/action_node.h"
#include "dictator_behavior_tree/pose_to_string.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using Nav2 = nav2_msgs::action::NavigateToPose;

namespace BT
{
class NavigateToPose : public RosActionNode<Nav2>
{
 public:
   NavigateToPose(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
       : RosActionNode<Nav2>(name, conf, params)
   {
   }

   static PortsList providedPorts()
   {
       return providedBasicPorts({InputPort<Position2D>("goal_pose")});
   }

   bool setGoal(RosActionNode::Goal &goal) override;

   NodeStatus onResultReceived(const WrappedResult &wr) override;

   NodeStatus onFailure(ActionNodeErrorCode error) override;

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

NodeStatus NavigateToPose::onFeedback(const std::shared_ptr<const Feedback> feedback)
{
    std::cout << "FEEDBACK" << std::endl;
    std::stringstream ss;
    ss << "Estimated remaining time: " << feedback->estimated_time_remaining.sec << " ";
    std::cout << ss.str() << std::endl;
    return NodeStatus::RUNNING;
}