simplify code

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

behaviortree_ros2/src/tree_execution_server.cpp

@@ -32,8 +32,6 @@ struct TreeExecutionServer::Pimpl
 {
  rclcpp_action::Server<ExecuteTree>::SharedPtr action_server;
  std::thread action_thread;
- // thread safe bool to keep track of cancel requests
- std::atomic<bool> cancel_requested{ false };
 
  std::shared_ptr<bt_server::ParamListener> param_listener;
  bt_server::Params params;
@@ -43,7 +41,7 @@ struct TreeExecutionServer::Pimpl
 
  std::string tree_name;
  std::string payload;
- std::shared_ptr<BT::Tree> 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,30 +168,29 @@ 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<BT::Tree>();
- *(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<BT::Groot2Publisher>(*(p_->tree), p_->params.groot2_port);
+ std::make_shared<BT::Groot2Publisher>(p_->tree, p_->params.groot2_port);
 
  // Loop until the tree is done or a cancel is requested
  const auto period =
  std::chrono::milliseconds(static_cast<int>(1000.0 / p_->params.tick_frequency));
  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,15 +200,15 @@ 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);
  return;
  }
 
  // 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()