From b0af5f657002a5614c9e16274b2ac78366fe0d78 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 7 May 2024 15:59:51 +0200 Subject: [PATCH 1/7] add payloads to Action --- .../tree_execution_server.hpp | 16 ++++-- .../src/tree_execution_server.cpp | 56 ++++++++++--------- .../action/ExecuteTree.action | 19 +++++-- btcpp_ros2_samples/src/sample_bt_executor.cpp | 4 +- 4 files changed, 59 insertions(+), 36 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index ca423b5..3be85d5 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -61,10 +61,13 @@ class TreeExecutionServer rclcpp::Node::SharedPtr node(); /// @brief Name of the tree being executed - const std::string& currentTreeName() const; + const std::string& treeName() const; + + /// @brief The payload received in the last goal + const std::string& goalPayload() const; /// @brief Tree being executed, nullptr if it doesn't exist, yet. - BT::Tree* currentTree(); + BT::Tree* tree(); /// @brief Pointer to the global blackboard BT::Blackboard::Ptr globalBlackboard(); @@ -110,9 +113,14 @@ class TreeExecutionServer * * @param status The status of the tree after the last tick * @param was_cancelled True if the action was cancelled by the Action Client + * + * @return if not std::nullopt, the string will be sent as [return_message] to the Action Client. */ - virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) - {} + virtual std::optional onTreeExecutionCompleted(BT::NodeStatus status, + bool was_cancelled) + { + return std::nullopt; + } /** * @brief onLoopFeedback is a callback invoked at each loop, after tree.tickOnce(). diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 420afa1..c17f360 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -41,7 +41,8 @@ struct TreeExecutionServer::Pimpl BT::BehaviorTreeFactory factory; std::shared_ptr groot_publisher; - std::string current_tree_name; + std::string tree_name; + std::string payload; std::shared_ptr tree; BT::Blackboard::Ptr global_blackboard; bool factory_initialized_ = false; @@ -173,7 +174,8 @@ void TreeExecutionServer::execute( p_->tree = std::make_shared(); *(p_->tree) = p_->factory.createTree(goal->target_tree, root_blackboard); - p_->current_tree_name = goal->target_tree; + p_->tree_name = goal->target_tree; + p_->payload = goal->payload; // call user defined function after the tree has been created onTreeCreated(*p_->tree); @@ -191,10 +193,14 @@ void TreeExecutionServer::execute( auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) { action_result->node_status = ConvertNodeStatus(status); - action_result->error_message = message; - RCLCPP_WARN(kLogger, action_result->error_message.c_str()); + action_result->return_message = message; + RCLCPP_WARN(kLogger, action_result->return_message.c_str()); p_->tree->haltTree(); - onTreeExecutionCompleted(status, true); + // override the message value if the user defined function returns it + if(auto msg = onTreeExecutionCompleted(status, true)) + { + action_result->return_message = msg.value(); + } }; while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) @@ -219,7 +225,7 @@ void TreeExecutionServer::execute( if(const auto res = onLoopFeedback(); res.has_value()) { auto feedback = std::make_shared(); - feedback->msg = res.value(); + feedback->message = res.value(); goal_handle->publish_feedback(feedback); } @@ -233,40 +239,38 @@ void TreeExecutionServer::execute( } catch(const std::exception& ex) { - action_result->error_message = std::string("Behavior Tree exception:") + ex.what(); - RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + action_result->return_message = std::string("Behavior Tree exception:") + ex.what(); + RCLCPP_ERROR(kLogger, action_result->return_message.c_str()); goal_handle->abort(action_result); return; } - // call user defined execution complete function - onTreeExecutionCompleted(status, false); - // set the node_status result to the action action_result->node_status = ConvertNodeStatus(status); - // return success or aborted for the action result - if(status == BT::NodeStatus::SUCCESS) - { - RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str()); - goal_handle->succeed(action_result); - } - else + // Call user defined onTreeExecutionCompleted function. + // Override the message value if the user defined function returns it + if(auto msg = onTreeExecutionCompleted(status, false)) { - action_result->error_message = std::string("Behavior Tree failed during execution " - "with status: ") + - BT::toStr(status); - RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); - goal_handle->abort(action_result); + action_result->return_message = msg.value(); } + + // return success or aborted for the action result + RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str()); + goal_handle->succeed(action_result); +} + +const std::string& TreeExecutionServer::treeName() const +{ + return p_->tree_name; } -const std::string& TreeExecutionServer::currentTreeName() const +const std::string& TreeExecutionServer::goalPayload() const { - return p_->current_tree_name; + return p_->payload; } -BT::Tree* TreeExecutionServer::currentTree() +BT::Tree* TreeExecutionServer::tree() { return p_->tree ? p_->tree.get() : nullptr; } diff --git a/btcpp_ros2_interfaces/action/ExecuteTree.action b/btcpp_ros2_interfaces/action/ExecuteTree.action index d12d6f6..5dee87d 100644 --- a/btcpp_ros2_interfaces/action/ExecuteTree.action +++ b/btcpp_ros2_interfaces/action/ExecuteTree.action @@ -1,9 +1,18 @@ -# Request +#### Request #### + +# Name of the tree to execute string target_tree +# Optional, implementation-dependent, payload. +string payload --- -# Result -string error_message +#### Result #### + +# Status of the tree NodeStatus node_status +# result payload or error message +string return_message --- -# Feedback. This can be customized by the user -string msg +#### Feedback #### + +#This can be customized by the user +string message diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index aa5b471..6b3e3a4 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -44,11 +44,13 @@ class MyActionServer : public BT::TreeExecutionServer logger_cout_ = std::make_shared(tree); } - void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override + std::optional onTreeExecutionCompleted(BT::NodeStatus status, + bool was_cancelled) override { // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree // at this point logger_cout_.reset(); + return std::nullopt; } private: From 06402c08e0010252fd9569372fda315d7e639846 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 8 May 2024 12:05:12 +0200 Subject: [PATCH 2/7] simplify code --- .../tree_execution_server.hpp | 4 +- .../src/tree_execution_server.cpp | 50 +++++++++++-------- 2 files changed, 31 insertions(+), 23 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 3be85d5..1760f44 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -66,8 +66,8 @@ class TreeExecutionServer /// @brief The payload received in the last goal const std::string& goalPayload() const; - /// @brief Tree being executed, nullptr if it doesn't exist, yet. - BT::Tree* tree(); + /// @brief Tree being executed. + const BT::Tree& tree() const; /// @brief Pointer to the global blackboard BT::Blackboard::Ptr globalBlackboard(); diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index c17f360..52b9438 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -32,8 +32,6 @@ struct TreeExecutionServer::Pimpl { rclcpp_action::Server::SharedPtr action_server; std::thread action_thread; - // thread safe bool to keep track of cancel requests - std::atomic cancel_requested{ false }; std::shared_ptr param_listener; bt_server::Params params; @@ -43,7 +41,7 @@ struct TreeExecutionServer::Pimpl std::string tree_name; std::string payload; - std::shared_ptr tree; + BT::Tree tree; BT::Blackboard::Ptr global_blackboard; bool factory_initialized_ = false; @@ -123,7 +121,6 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, { RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", goal->target_tree.c_str()); - p_->cancel_requested = false; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -137,7 +134,6 @@ rclcpp_action::CancelResponse TreeExecutionServer::handle_cancel( "processing one."); return rclcpp_action::CancelResponse::REJECT; } - p_->cancel_requested = true; return rclcpp_action::CancelResponse::ACCEPT; } @@ -172,16 +168,15 @@ void TreeExecutionServer::execute( // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard auto root_blackboard = BT::Blackboard::create(p_->global_blackboard); - p_->tree = std::make_shared(); - *(p_->tree) = p_->factory.createTree(goal->target_tree, root_blackboard); + p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard); p_->tree_name = goal->target_tree; p_->payload = goal->payload; // call user defined function after the tree has been created - onTreeCreated(*p_->tree); + onTreeCreated(p_->tree); p_->groot_publisher.reset(); p_->groot_publisher = - std::make_shared(*(p_->tree), p_->params.groot2_port); + std::make_shared(p_->tree, p_->params.groot2_port); // Loop until the tree is done or a cancel is requested const auto period = @@ -189,13 +184,13 @@ void TreeExecutionServer::execute( auto loop_deadline = std::chrono::steady_clock::now() + period; // operations to be done if the tree execution is aborted, either by - // cancel_requested_ or by onLoopAfterTick() + // cancel requested or by onLoopAfterTick() auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) { action_result->node_status = ConvertNodeStatus(status); action_result->return_message = message; RCLCPP_WARN(kLogger, action_result->return_message.c_str()); - p_->tree->haltTree(); + p_->tree.haltTree(); // override the message value if the user defined function returns it if(auto msg = onTreeExecutionCompleted(status, true)) { @@ -205,7 +200,7 @@ void TreeExecutionServer::execute( while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) { - if(p_->cancel_requested) + if(goal_handle->is_canceling()) { stop_action(status, "Action Server canceling and halting Behavior Tree"); goal_handle->canceled(action_result); @@ -213,7 +208,7 @@ void TreeExecutionServer::execute( } // tick the tree once and publish the action feedback - status = p_->tree->tickExactlyOnce(); + status = p_->tree.tickExactlyOnce(); if(const auto res = onLoopAfterTick(status); res.has_value()) { @@ -232,7 +227,7 @@ void TreeExecutionServer::execute( const auto now = std::chrono::steady_clock::now(); if(now < loop_deadline) { - p_->tree->sleep(loop_deadline - now); + p_->tree.sleep(loop_deadline - now); } loop_deadline += period; } @@ -245,19 +240,32 @@ void TreeExecutionServer::execute( return; } - // set the node_status result to the action - action_result->node_status = ConvertNodeStatus(status); - // Call user defined onTreeExecutionCompleted function. // Override the message value if the user defined function returns it if(auto msg = onTreeExecutionCompleted(status, false)) { action_result->return_message = msg.value(); } + else + { + action_result->return_message = + std::string("Tree finished with status: ") + BT::toStr(status); + } + + // set the node_status result to the action + action_result->node_status = ConvertNodeStatus(status); // return success or aborted for the action result - RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str()); - goal_handle->succeed(action_result); + if(status == BT::NodeStatus::SUCCESS) + { + RCLCPP_INFO(kLogger, action_result->return_message.c_str()); + goal_handle->succeed(action_result); + } + else + { + RCLCPP_ERROR(kLogger, action_result->return_message.c_str()); + goal_handle->abort(action_result); + } } const std::string& TreeExecutionServer::treeName() const @@ -270,9 +278,9 @@ const std::string& TreeExecutionServer::goalPayload() const return p_->payload; } -BT::Tree* TreeExecutionServer::tree() +const BT::Tree& TreeExecutionServer::tree() const { - return p_->tree ? p_->tree.get() : nullptr; + return p_->tree; } BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() From 6df134a07f56f0e7ec55d58e4aa08d61c0cfb346 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 8 May 2024 12:12:39 +0200 Subject: [PATCH 3/7] minor change in log --- behaviortree_ros2/src/tree_execution_server.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 52b9438..80c112a 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -187,15 +187,18 @@ void TreeExecutionServer::execute( // cancel requested or by onLoopAfterTick() auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) { - action_result->node_status = ConvertNodeStatus(status); - action_result->return_message = message; - RCLCPP_WARN(kLogger, action_result->return_message.c_str()); p_->tree.haltTree(); + action_result->node_status = ConvertNodeStatus(status); // override the message value if the user defined function returns it if(auto msg = onTreeExecutionCompleted(status, true)) { action_result->return_message = msg.value(); } + else + { + action_result->return_message = message; + } + RCLCPP_WARN(kLogger, action_result->return_message.c_str()); }; while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) From d567cc837697bacd5b0b771d44146b2e540c41a1 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 10 May 2024 13:24:11 +0200 Subject: [PATCH 4/7] add callback onGoalReceived --- .../include/behaviortree_ros2/tree_execution_server.hpp | 9 +++++++++ behaviortree_ros2/src/tree_execution_server.cpp | 6 ++++++ 2 files changed, 15 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 1760f44..f472cfd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -76,6 +76,15 @@ class TreeExecutionServer BT::BehaviorTreeFactory& factory(); protected: + /** + * @brief Callback invoked when a goal is received and before the tree is created. + * If it returns false, the goal will be rejected. + */ + virtual bool onGoalReceived(const std::string& tree_name, const std::string& payload) + { + return true; + } + /** * @brief Callback invoked after the tree is created. * It can be used, for instance, to initialize a logger or the global blackboard. diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 80c112a..2f68a3c 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -121,6 +121,12 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, { RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", goal->target_tree.c_str()); + + if(!onGoalReceived(goal->target_tree, goal->payload)) + { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } From 25434443cf5d6a673a7503ed4c35d635450413e3 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 10 May 2024 13:24:33 +0200 Subject: [PATCH 5/7] set bool examples --- btcpp_ros2_samples/CMakeLists.txt | 12 +++++ btcpp_ros2_samples/src/bool_client.cpp | 33 ++++++++++++ btcpp_ros2_samples/src/bool_server.cpp | 37 ++++++++++++++ btcpp_ros2_samples/src/set_bool_node.cpp | 64 ++++++++++++++++++++++++ btcpp_ros2_samples/src/set_bool_node.hpp | 52 +++++++++++++++++++ 5 files changed, 198 insertions(+) create mode 100644 btcpp_ros2_samples/src/bool_client.cpp create mode 100644 btcpp_ros2_samples/src/bool_server.cpp create mode 100644 btcpp_ros2_samples/src/set_bool_node.cpp create mode 100644 btcpp_ros2_samples/src/set_bool_node.hpp diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 785d674..0ff08e5 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -9,10 +9,12 @@ find_package(ament_cmake REQUIRED) find_package(behaviortree_ros2 REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) set(THIS_PACKAGE_DEPS behaviortree_ros2 std_msgs + std_srvs btcpp_ros2_interfaces ) ###################################################### @@ -50,6 +52,14 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) add_executable(subscriber_test src/subscriber_test.cpp) ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) +###################################################### +# the SetBool test +add_executable(bool_client src/bool_client.cpp src/set_bool_node.cpp) +ament_target_dependencies(bool_client ${THIS_PACKAGE_DEPS}) + +add_executable(bool_server src/bool_server.cpp ) +ament_target_dependencies(bool_server ${THIS_PACKAGE_DEPS}) + ###################################################### # INSTALL @@ -60,6 +70,8 @@ install(TARGETS sleep_plugin subscriber_test sample_bt_executor + bool_client + bool_server DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/bool_client.cpp b/btcpp_ros2_samples/src/bool_client.cpp new file mode 100644 index 0000000..0fe5c35 --- /dev/null +++ b/btcpp_ros2_samples/src/bool_client.cpp @@ -0,0 +1,33 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +#include "set_bool_node.hpp" + +using namespace BT; + +static const char* xml_text = R"( + + + + + + + + + )"; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto nh = std::make_shared("bool_client"); + + BehaviorTreeFactory factory; + factory.registerNodeType("RobotSetBool", "set_bool", nh); + + auto tree = factory.createTreeFromText(xml_text); + + tree.tickWhileRunning(); + + return 0; +} diff --git a/btcpp_ros2_samples/src/bool_server.cpp b/btcpp_ros2_samples/src/bool_server.cpp new file mode 100644 index 0000000..91c77e8 --- /dev/null +++ b/btcpp_ros2_samples/src/bool_server.cpp @@ -0,0 +1,37 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include + +void CallbackA(const std::shared_ptr request, + std::shared_ptr response) +{ + response->success = request->data; + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_INFO(logger, "Incoming request A: %s", request->data ? "true" : "false"); +} + +void CallbackB(const std::shared_ptr request, + std::shared_ptr response) +{ + response->success = request->data; + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_INFO(logger, "Incoming request B: %s", request->data ? "true" : "false"); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + auto serviceA = + node->create_service("robotA/set_bool", &CallbackA); + auto serviceB = + node->create_service("robotB/set_bool", &CallbackB); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/btcpp_ros2_samples/src/set_bool_node.cpp b/btcpp_ros2_samples/src/set_bool_node.cpp new file mode 100644 index 0000000..c5554ac --- /dev/null +++ b/btcpp_ros2_samples/src/set_bool_node.cpp @@ -0,0 +1,64 @@ +#include "set_bool_node.hpp" + +bool SetBoolService::setRequest(Request::SharedPtr& request) +{ + getInput("value", request->data); + std::cout << "setRequest " << std::endl; + return true; +} + +BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& response) +{ + std::cout << "onResponseReceived " << std::endl; + if(response->success) + { + RCLCPP_INFO(logger(), "SetString service succeeded."); + return BT::NodeStatus::SUCCESS; + } + else + { + RCLCPP_INFO(logger(), "SetString service failed: %s", response->message.c_str()); + return BT::NodeStatus::FAILURE; + } +} + +BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "Error: %d", error); + return BT::NodeStatus::FAILURE; +} + +//------------------------------------------------------ +//------------------------------------------------------ +//------------------------------------------------------ + +NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, + const std::string& service_name, + std::weak_ptr node) + : BT::ActionNodeBase(name, conf) + , local_bb_(BT::Blackboard::create(conf.blackboard)) + , service_name_(service_name) +{ + BT::NodeConfig impl_config; + impl_config.blackboard = local_bb_; + impl_config.input_ports["service_name"] = "{=}"; + impl_config.input_ports["value"] = "{=}"; + BT::RosNodeParams impl_params; + impl_params.nh = node; + set_bool_service_ = std::make_unique(name, impl_config, impl_params); +} + +BT::NodeStatus NamespacedSetBool::tick() +{ + std::string robot; + bool command; + if(!getInput("robot", robot) || !getInput("command", command)) + { + throw BT::RuntimeError("NamespacedSetBool: Missing inputs"); + } + + local_bb_->set("service_name", robot + "/" + service_name_); + local_bb_->set("value", command); + std::cout << "ticking " << std::endl; + return set_bool_service_->tick(); +} diff --git a/btcpp_ros2_samples/src/set_bool_node.hpp b/btcpp_ros2_samples/src/set_bool_node.hpp new file mode 100644 index 0000000..f0f0c72 --- /dev/null +++ b/btcpp_ros2_samples/src/set_bool_node.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include "std_srvs/srv/set_bool.hpp" + +using SetBool = std_srvs::srv::SetBool; + +class SetBoolService : public BT::RosServiceNode +{ +public: + SetBoolService(const std::string& name, const BT::NodeConfig& conf, + const BT::RosNodeParams& params) + : RosServiceNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ BT::InputPort("value") }); + } + + bool setRequest(Request::SharedPtr& request) override; + + BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override; + + virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override; +}; + +//------------------------------------------------------ + +class NamespacedSetBool : public BT::ActionNodeBase +{ +public: + NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, + const std::string& service_name, std::weak_ptr node); + + static BT::PortsList providedPorts() + { + return { BT::InputPort("robot"), BT::InputPort("command") }; + } + + BT::NodeStatus tick() override; + + void halt() override + { + set_bool_service_->halt(); + } + +private: + BT::Blackboard::Ptr local_bb_; + std::string service_name_; + std::unique_ptr set_bool_service_; +}; From 0582a8686b1cd650189fe33800d9c6fd3638ddb4 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 10 May 2024 13:38:32 +0200 Subject: [PATCH 6/7] updated --- btcpp_ros2_samples/src/set_bool_node.cpp | 19 ++++++++++++++++++- btcpp_ros2_samples/src/set_bool_node.hpp | 19 ++++++++++++++++++- 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/btcpp_ros2_samples/src/set_bool_node.cpp b/btcpp_ros2_samples/src/set_bool_node.cpp index c5554ac..5d02b99 100644 --- a/btcpp_ros2_samples/src/set_bool_node.cpp +++ b/btcpp_ros2_samples/src/set_bool_node.cpp @@ -32,9 +32,25 @@ BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error) //------------------------------------------------------ //------------------------------------------------------ +NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, + const BT::RosNodeParams& params) + : BT::ActionNodeBase(name, conf) + , local_bb_(BT::Blackboard::create(conf.blackboard)) + , service_name_(params.default_port_value) +{ + BT::NodeConfig impl_config; + impl_config.blackboard = local_bb_; + impl_config.input_ports["service_name"] = "{=}"; + impl_config.input_ports["value"] = "{=}"; + + BT::RosNodeParams impl_params = params; + impl_params.default_port_value = {}; // postpone this + set_bool_service_ = std::make_unique(name, impl_config, impl_params); +} + NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, const std::string& service_name, - std::weak_ptr node) + rclcpp::Node::SharedPtr node) : BT::ActionNodeBase(name, conf) , local_bb_(BT::Blackboard::create(conf.blackboard)) , service_name_(service_name) @@ -43,6 +59,7 @@ NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConf impl_config.blackboard = local_bb_; impl_config.input_ports["service_name"] = "{=}"; impl_config.input_ports["value"] = "{=}"; + BT::RosNodeParams impl_params; impl_params.nh = node; set_bool_service_ = std::make_unique(name, impl_config, impl_params); diff --git a/btcpp_ros2_samples/src/set_bool_node.hpp b/btcpp_ros2_samples/src/set_bool_node.hpp index f0f0c72..09b0603 100644 --- a/btcpp_ros2_samples/src/set_bool_node.hpp +++ b/btcpp_ros2_samples/src/set_bool_node.hpp @@ -27,11 +27,28 @@ class SetBoolService : public BT::RosServiceNode //------------------------------------------------------ +// This is a workaround that can be used when we want to use SetBoolService, +// but the service name has a namespace. +// Therefore, instead of: +// +// +// +// We can rewrite it as: +// +// +// +// Registration in C++: +// +// factory.registerNodeType("SetRobotBool", "set_bool", node); + class NamespacedSetBool : public BT::ActionNodeBase { public: NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, - const std::string& service_name, std::weak_ptr node); + const BT::RosNodeParams& params); + + NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, + const std::string& service_name, rclcpp::Node::SharedPtr node); static BT::PortsList providedPorts() { From bc5297ebf9b3232c531f6e441e2d67902686f04e Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 15 May 2024 17:53:31 +0200 Subject: [PATCH 7/7] make it easier to change action/service name programmatically --- .../behaviortree_ros2/bt_action_node.hpp | 70 +++++++++---------- .../behaviortree_ros2/bt_service_node.hpp | 66 ++++++++--------- .../behaviortree_ros2/ros_node_params.hpp | 7 ++ btcpp_ros2_samples/src/bool_client.cpp | 18 ++++- btcpp_ros2_samples/src/set_bool_node.cpp | 54 ++------------ btcpp_ros2_samples/src/set_bool_node.hpp | 46 ++++-------- 6 files changed, 106 insertions(+), 155 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index e2d4f15..05c664d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -108,8 +108,7 @@ class RosActionNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { InputPort("action_name", "__default__placeholder__", - "Action server name") }; + PortsList basic = { InputPort("action_name", "", "Action server name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -164,9 +163,12 @@ class RosActionNode : public BT::ActionNodeBase void cancelGoal(); /// The default halt() implementation will call cancelGoal if necessary. - void halt() override final; + void halt() override; - NodeStatus tick() override final; + NodeStatus tick() override; + + /// Can be used to change the name of the action programmatically + void setActionName(const std::string& action_name); protected: struct ActionClientInstance @@ -216,7 +218,7 @@ class RosActionNode : public BT::ActionNodeBase std::weak_ptr node_; std::shared_ptr client_instance_; std::string action_name_; - bool action_name_may_change_ = false; + bool action_name_should_be_checked_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; std::string action_client_key_; @@ -265,44 +267,23 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, auto portIt = config().input_ports.find("action_name"); if(portIt != config().input_ports.end()) { - const std::string& bb_action_name = portIt->second; + const std::string& bb_service_name = portIt->second; - if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [action_name] in the InputPort and the " - "RosNodeParams are empty."); - } - else - { - createClient(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_action_name)) + if(isBlackboardPointer(bb_service_name)) { - // If the content of the port "action_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createClient(bb_action_name); + // unknown value at construction time. Postpone to tick + action_name_should_be_checked_ = true; } - else + else if(!bb_service_name.empty()) { - action_name_may_change_ = true; - // createClient will be invoked in the first tick(). + // "hard-coded" name in the bb_service_name. Use it. + createClient(bb_service_name); } } - else + // no port value or it is empty. Use the default value + if(!client_instance_ && !params.default_port_value.empty()) { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [action_name] in the InputPort and the RosNodeParams " - "are empty."); - } - else - { - createClient(params.default_port_value); - } + createClient(params.default_port_value); } } @@ -347,6 +328,13 @@ inline bool RosActionNode::createClient(const std::string& action_name) return found; } +template +inline void RosActionNode::setActionName(const std::string& action_name) +{ + action_name_ = action_name; + createClient(action_name); +} + template inline NodeStatus RosActionNode::tick() { @@ -359,7 +347,8 @@ inline NodeStatus RosActionNode::tick() // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client - if(!client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + if(!client_instance_ || + (status() == NodeStatus::IDLE && action_name_should_be_checked_)) { std::string action_name; getInput("action_name", action_name); @@ -368,6 +357,13 @@ inline NodeStatus RosActionNode::tick() createClient(action_name); } } + + if(!client_instance_) + { + throw BT::RuntimeError("RosActionNode: no client was specified neither as default or " + "in the ports"); + } + auto& action_client = client_instance_->action_client; //------------------------------------------ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index f3a6203..77958cd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -96,8 +96,7 @@ class RosServiceNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { InputPort("service_name", "__default__placeholder__", - "Service name") }; + PortsList basic = { InputPort("service_name", "", "Service name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -111,7 +110,7 @@ class RosServiceNode : public BT::ActionNodeBase return providedBasicPorts({}); } - NodeStatus tick() override final; + NodeStatus tick() override; /// The default halt() implementation. void halt() override; @@ -157,6 +156,9 @@ class RosServiceNode : public BT::ActionNodeBase return action_client_mutex; } + // method to set the service name programmatically + void setServiceName(const std::string& service_name); + rclcpp::Logger logger() { if(auto node = node_.lock()) @@ -186,7 +188,7 @@ class RosServiceNode : public BT::ActionNodeBase std::weak_ptr node_; std::string service_name_; - bool service_name_may_change_ = false; + bool service_name_should_be_checked_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; @@ -233,51 +235,30 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, { const std::string& bb_service_name = portIt->second; - if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") + if(isBlackboardPointer(bb_service_name)) { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [service_name] in the InputPort and the " - "RosNodeParams are empty."); - } - else - { - createClient(params.default_port_value); - } + // unknown value at construction time. postpone to tick + service_name_should_be_checked_ = true; } - else if(!isBlackboardPointer(bb_service_name)) + else if(!bb_service_name.empty()) { - // If the content of the port "service_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. + // "hard-coded" name in the bb_service_name. Use it. createClient(bb_service_name); } - else - { - service_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } } - else + // no port value or it is empty. Use the default port value + if(!srv_instance_ && !params.default_port_value.empty()) { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [service_name] in the InputPort and the RosNodeParams " - "are empty."); - } - else - { - createClient(params.default_port_value); - } + createClient(params.default_port_value); } } template inline bool RosServiceNode::createClient(const std::string& service_name) { - if(service_name.empty()) + if(service_name.empty() || service_name == "__default__placeholder__") { - throw RuntimeError("service_name is empty"); + throw RuntimeError("service_name is empty or invalid"); } std::unique_lock lk(getMutex()); @@ -314,6 +295,13 @@ inline bool RosServiceNode::createClient(const std::string& service_name) return found; } +template +inline void RosServiceNode::setServiceName(const std::string& service_name) +{ + service_name_ = service_name; + createClient(service_name); +} + template inline NodeStatus RosServiceNode::tick() { @@ -326,7 +314,7 @@ inline NodeStatus RosServiceNode::tick() // First, check if the service_client is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client - if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_should_be_checked_)) { std::string service_name; getInput("service_name", service_name); @@ -336,6 +324,12 @@ inline NodeStatus RosServiceNode::tick() } } + if(!srv_instance_) + { + throw BT::RuntimeError("RosServiceNode: no service client was specified neither as " + "default or in the ports"); + } + auto CheckStatus = [](NodeStatus status) { if(!isStatusCompleted(status)) { diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index 484429b..d9e1de7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -40,6 +40,13 @@ struct RosNodeParams std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); // timeout used when detecting the server the first time std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); + + RosNodeParams() = default; + RosNodeParams(std::shared_ptr node) : nh(node) + {} + RosNodeParams(std::shared_ptr node, const std::string& port_name) + : nh(node), default_port_value(port_name) + {} }; } // namespace BT diff --git a/btcpp_ros2_samples/src/bool_client.cpp b/btcpp_ros2_samples/src/bool_client.cpp index 0fe5c35..dc00710 100644 --- a/btcpp_ros2_samples/src/bool_client.cpp +++ b/btcpp_ros2_samples/src/bool_client.cpp @@ -10,8 +10,11 @@ static const char* xml_text = R"( - - + + + + + @@ -23,7 +26,16 @@ int main(int argc, char** argv) auto nh = std::make_shared("bool_client"); BehaviorTreeFactory factory; - factory.registerNodeType("RobotSetBool", "set_bool", nh); + + // version with default port + factory.registerNodeType("SetBoolA", BT::RosNodeParams(nh, "robotA/" + "set_bool")); + + // version without default port + factory.registerNodeType("SetBool", BT::RosNodeParams(nh)); + + // namespace version + factory.registerNodeType("SetRobotBool", nh, "set_bool"); auto tree = factory.createTreeFromText(xml_text); diff --git a/btcpp_ros2_samples/src/set_bool_node.cpp b/btcpp_ros2_samples/src/set_bool_node.cpp index 5d02b99..8d8240d 100644 --- a/btcpp_ros2_samples/src/set_bool_node.cpp +++ b/btcpp_ros2_samples/src/set_bool_node.cpp @@ -12,12 +12,12 @@ BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& res std::cout << "onResponseReceived " << std::endl; if(response->success) { - RCLCPP_INFO(logger(), "SetString service succeeded."); + RCLCPP_INFO(logger(), "SetBool service succeeded."); return BT::NodeStatus::SUCCESS; } else { - RCLCPP_INFO(logger(), "SetString service failed: %s", response->message.c_str()); + RCLCPP_INFO(logger(), "SetBool service failed: %s", response->message.c_str()); return BT::NodeStatus::FAILURE; } } @@ -28,54 +28,14 @@ BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error) return BT::NodeStatus::FAILURE; } -//------------------------------------------------------ -//------------------------------------------------------ -//------------------------------------------------------ +//----------------------------------------------------------- -NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, - const BT::RosNodeParams& params) - : BT::ActionNodeBase(name, conf) - , local_bb_(BT::Blackboard::create(conf.blackboard)) - , service_name_(params.default_port_value) -{ - BT::NodeConfig impl_config; - impl_config.blackboard = local_bb_; - impl_config.input_ports["service_name"] = "{=}"; - impl_config.input_ports["value"] = "{=}"; - - BT::RosNodeParams impl_params = params; - impl_params.default_port_value = {}; // postpone this - set_bool_service_ = std::make_unique(name, impl_config, impl_params); -} - -NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, - const std::string& service_name, - rclcpp::Node::SharedPtr node) - : BT::ActionNodeBase(name, conf) - , local_bb_(BT::Blackboard::create(conf.blackboard)) - , service_name_(service_name) -{ - BT::NodeConfig impl_config; - impl_config.blackboard = local_bb_; - impl_config.input_ports["service_name"] = "{=}"; - impl_config.input_ports["value"] = "{=}"; - - BT::RosNodeParams impl_params; - impl_params.nh = node; - set_bool_service_ = std::make_unique(name, impl_config, impl_params); -} - -BT::NodeStatus NamespacedSetBool::tick() +BT::NodeStatus SetRobotBoolService::tick() { std::string robot; - bool command; - if(!getInput("robot", robot) || !getInput("command", command)) + if(getInput("robot", robot) && !robot.empty()) { - throw BT::RuntimeError("NamespacedSetBool: Missing inputs"); + setServiceName(robot + "/" + service_suffix_); } - - local_bb_->set("service_name", robot + "/" + service_name_); - local_bb_->set("value", command); - std::cout << "ticking " << std::endl; - return set_bool_service_->tick(); + return SetBoolService::tick(); } diff --git a/btcpp_ros2_samples/src/set_bool_node.hpp b/btcpp_ros2_samples/src/set_bool_node.hpp index 09b0603..86a12d3 100644 --- a/btcpp_ros2_samples/src/set_bool_node.hpp +++ b/btcpp_ros2_samples/src/set_bool_node.hpp @@ -8,8 +8,8 @@ using SetBool = std_srvs::srv::SetBool; class SetBoolService : public BT::RosServiceNode { public: - SetBoolService(const std::string& name, const BT::NodeConfig& conf, - const BT::RosNodeParams& params) + explicit SetBoolService(const std::string& name, const BT::NodeConfig& conf, + const BT::RosNodeParams& params) : RosServiceNode(name, conf, params) {} @@ -23,47 +23,29 @@ class SetBoolService : public BT::RosServiceNode BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override; virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override; -}; -//------------------------------------------------------ +private: + std::string service_suffix_; +}; -// This is a workaround that can be used when we want to use SetBoolService, -// but the service name has a namespace. -// Therefore, instead of: -// -// -// -// We can rewrite it as: -// -// -// -// Registration in C++: -// -// factory.registerNodeType("SetRobotBool", "set_bool", node); +//---------------------------------------------- -class NamespacedSetBool : public BT::ActionNodeBase +class SetRobotBoolService : public SetBoolService { public: - NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, - const BT::RosNodeParams& params); - - NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf, - const std::string& service_name, rclcpp::Node::SharedPtr node); + explicit SetRobotBoolService(const std::string& name, const BT::NodeConfig& conf, + const rclcpp::Node::SharedPtr& node, + const std::string& port_name) + : SetBoolService(name, conf, BT::RosNodeParams(node)), service_suffix_(port_name) + {} static BT::PortsList providedPorts() { - return { BT::InputPort("robot"), BT::InputPort("command") }; + return { BT::InputPort("robot"), BT::InputPort("value") }; } BT::NodeStatus tick() override; - void halt() override - { - set_bool_service_->halt(); - } - private: - BT::Blackboard::Ptr local_bb_; - std::string service_name_; - std::unique_ptr set_bool_service_; + std::string service_suffix_; };