Method execute

behaviortree_ros2/src/tree_execution_server.cpp:176–297

}

void TreeExecutionServer::execute(
    const std::shared_ptr<GoalHandleExecuteTree> goal_handle)
{
  const auto goal = goal_handle->get_goal();
  BT::NodeStatus status = BT::NodeStatus::RUNNING;
  auto action_result = std::make_shared<ExecuteTree::Result>();

  // Before executing check if we have new Behaviors or Subtrees to reload
  if(p_->param_listener->is_old(p_->params))
  {
    executeRegistration();
  }

  // Loop until something happens with ROS or the node completes
  try
  {
    // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard
    auto root_blackboard = BT::Blackboard::create(p_->global_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);
    p_->groot_publisher.reset();
    p_->groot_publisher =
        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()
    auto stop_action = [this, &action_result](BT::NodeStatus status,
                                              const std::string& message) {
      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)
    {
      if(goal_handle->is_canceling())
      {
        stop_action(status, "Action Server canceling and halting Behavior Tree");
        goal_handle->canceled(action_result);