From b0af5f657002a5614c9e16274b2ac78366fe0d78 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 7 May 2024 15:59:51 +0200 Subject: [PATCH 1/3] 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/3] 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/3] 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)