diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index ca423b5..1760f44 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 Tree being executed, nullptr if it doesn't exist, yet. - BT::Tree* currentTree(); + /// @brief The payload received in the last goal + const std::string& goalPayload() const; + + /// @brief Tree being executed. + const BT::Tree& tree() const; /// @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..80c112a 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; @@ -41,8 +39,9 @@ struct TreeExecutionServer::Pimpl BT::BehaviorTreeFactory factory; std::shared_ptr groot_publisher; - std::string current_tree_name; - std::shared_ptr tree; + std::string tree_name; + std::string payload; + BT::Tree tree; BT::Blackboard::Ptr global_blackboard; bool factory_initialized_ = false; @@ -122,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; } @@ -136,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; } @@ -171,15 +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_->current_tree_name = goal->target_tree; + 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 = @@ -187,19 +184,26 @@ 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) { + p_->tree.haltTree(); action_result->node_status = ConvertNodeStatus(status); - action_result->error_message = message; - RCLCPP_WARN(kLogger, action_result->error_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(); + } + else + { + action_result->return_message = message; + } + RCLCPP_WARN(kLogger, action_result->return_message.c_str()); }; 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); @@ -207,7 +211,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()) { @@ -219,28 +223,37 @@ 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); } 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; } } 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); + // 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); @@ -248,27 +261,29 @@ void TreeExecutionServer::execute( // 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()); + RCLCPP_INFO(kLogger, action_result->return_message.c_str()); goal_handle->succeed(action_result); } else { - 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()); + RCLCPP_ERROR(kLogger, action_result->return_message.c_str()); goal_handle->abort(action_result); } } -const std::string& TreeExecutionServer::currentTreeName() const +const std::string& TreeExecutionServer::treeName() const +{ + return p_->tree_name; +} + +const std::string& TreeExecutionServer::goalPayload() const { - return p_->current_tree_name; + return p_->payload; } -BT::Tree* TreeExecutionServer::currentTree() +const BT::Tree& TreeExecutionServer::tree() const { - return p_->tree ? p_->tree.get() : nullptr; + return p_->tree; } BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() 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: