From 06402c08e0010252fd9569372fda315d7e639846 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 8 May 2024 12:05:12 +0200 Subject: [PATCH] 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()