bt_action_node.hpp

// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
 
#include <memory>
#include <string>
#include <chrono>
#include <cstdint>
 
#include "behaviortree_cpp/action_node.h"
#include "behaviortree_cpp/json_export.h"
#include "nav2_ros_common/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behavior_tree/bt_utils.hpp"
#include "nav2_behavior_tree/json_utils.hpp"
 
namespace nav2_behavior_tree
{
 
using namespace std::chrono_literals;  // NOLINT
 
template<class ActionT>
class BtActionNode : public BT::ActionNodeBase
{
public:
  using ActionResult = typename ActionT::Result;
 
  BtActionNode(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf)
  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true)
  {
    node_ = config().blackboard->template get<nav2::LifecycleNode::SharedPtr>("node");
    callback_group_ = node_->create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive,
      false);
    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
    // Get the required items from the blackboard
    auto bt_loop_duration =
      config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
    getInputOrBlackboard("server_timeout", server_timeout_);
    getInputOrBlackboard("cancel_timeout", cancel_timeout_);
    wait_for_service_timeout_ =
      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
    // timeout should be less than bt_loop_duration to be able to finish the current tick
    max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
 
    // Initialize the input and output messages
    goal_ = typename ActionT::Goal();
    result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
 
    if constexpr (
      !requires {ActionT::Result::TIMEOUT;} ||
      !requires {ActionT::Result::GOAL_REJECTED;} ||
      !requires {ActionT::Result::SEND_GOAL_FAILURE;})
    {
      if constexpr (requires {ActionT::Result::UNKNOWN;}) {
        RCLCPP_WARN(
          node_->get_logger(),
          "Action type for \"%s\" does not define one or more of the TIMEOUT, "
          "GOAL_REJECTED, and SEND_GOAL_FAILURE error codes. UNKNOWN will be "
          "used for unavailable errors.",
          xml_tag_name.c_str());
      } else {
        RCLCPP_WARN(
          node_->get_logger(),
          "Action type for \"%s\" does not define one or more of the TIMEOUT, "
          "GOAL_REJECTED, and SEND_GOAL_FAILURE error codes. The error_code_id "
          "output will not be set for unavailable errors.",
          xml_tag_name.c_str());
      }
    }
 
    std::string remapped_action_name;
    if (getInput("server_name", remapped_action_name)) {
      action_name_ = remapped_action_name;
    }
    createActionClient(action_name_);
 
    // Give the derive class a chance to do any initialization
    RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
  }
 
  BtActionNode() = delete;
 
  virtual ~BtActionNode()
  {
  }
 
  void createActionClient(const std::string & action_name)
  {
    // Now that we have the ROS node to use, create the action client for this BT action
    action_client_ = node_->create_action_client<ActionT>(action_name, callback_group_);
 
    // Make sure the server is actually there before continuing
    RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
    if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
      RCLCPP_ERROR(
        node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
        action_name.c_str(),
        wait_for_service_timeout_.count() / 1000.0);
      throw std::runtime_error(
              std::string("Action server ") + action_name +
              std::string(" not available"));
    }
  }
 
  static BT::PortsList providedBasicPorts(BT::PortsList addition)
  {
    BT::PortsList basic = {
      BT::InputPort<std::string>("server_name", "Action server name"),
      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
      BT::OutputPort<uint16_t>("error_code_id", "The action error code"),
      BT::OutputPort<std::string>("error_msg", "The action error message")
    };
    basic.insert(addition.begin(), addition.end());
 
    return basic;
  }
 
  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({});
  }
 
  // Derived classes can override any of the following methods to hook into the
  // processing for the action: on_tick, on_wait_for_result, and on_success
 
  virtual void on_tick()
  {
  }
 
  virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>/*feedback*/)
  {
  }
 
  virtual BT::NodeStatus on_success()
  {
    return BT::NodeStatus::SUCCESS;
  }
 
  virtual BT::NodeStatus on_aborted()
  {
    return BT::NodeStatus::FAILURE;
  }
 
  virtual BT::NodeStatus on_cancelled()
  {
    return BT::NodeStatus::SUCCESS;
  }
 
  virtual void on_timeout()
  {
    if constexpr (requires {ActionT::Result::TIMEOUT;}) {
      setOutput("error_code_id", ActionResult::TIMEOUT);
    } else if constexpr (requires {ActionT::Result::UNKNOWN;}) {
      setOutput("error_code_id", ActionResult::UNKNOWN);
    }
    setOutput("error_msg", "Behavior Tree action client timed out waiting.");
  }
 
  virtual void on_goal_rejected()
  {
    if constexpr (requires {ActionT::Result::GOAL_REJECTED;}) {
      setOutput("error_code_id", ActionResult::GOAL_REJECTED);
    } else if constexpr (requires {ActionT::Result::UNKNOWN;}) {
      setOutput("error_code_id", ActionResult::UNKNOWN);
    }
    setOutput("error_msg", "Goal was rejected by the action server.");
  }
 
  virtual void on_send_goal_failure()
  {
    if constexpr (requires {ActionT::Result::SEND_GOAL_FAILURE;}) {
      setOutput("error_code_id", ActionResult::SEND_GOAL_FAILURE);
    } else if constexpr (requires {ActionT::Result::UNKNOWN;}) {
      setOutput("error_code_id", ActionResult::UNKNOWN);
    }
    setOutput("error_msg", "Failed to send goal to the action server.");
  }
 
  BT::NodeStatus tick() override
  {
    // first step to be done only at the beginning of the Action
    if (!BT::isStatusActive(status())) {
      // reset the flag to send the goal or not, allowing the user the option to set it in on_tick
      should_send_goal_ = true;
 
      // Clear the input and output messages to make sure we have no leftover from previous calls
      goal_ = typename ActionT::Goal();
      result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
 
      // user defined callback, may modify "should_send_goal_".
      on_tick();
 
      // setting the status to RUNNING to notify the BT Loggers (if any)
      setStatus(BT::NodeStatus::RUNNING);
 
      if (!should_send_goal_) {
        return BT::NodeStatus::FAILURE;
      }
      send_new_goal();
    }
 
    try {
      // if new goal was sent and action server has not yet responded
      // check the future goal handle
      if (future_goal_handle_) {
        auto elapsed =
          (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
        if (!is_future_goal_handle_complete(elapsed)) {
          // return RUNNING if there is still some time before timeout happens
          if (elapsed < server_timeout_) {
            return BT::NodeStatus::RUNNING;
          }
          handle_goal_response_timeout();
          return BT::NodeStatus::FAILURE;
        }
      }
 
      // The following code corresponds to the "RUNNING" loop
      if (rclcpp::ok() && !goal_result_available_) {
        // user defined callback. May modify the value of "goal_updated_"
        on_wait_for_result(feedback_);
 
        // reset feedback to avoid stale information
        feedback_.reset();
 
        auto goal_status = goal_handle_->get_status();
        if (goal_updated_ &&
          (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
          goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
        {
          goal_updated_ = false;
          send_new_goal();
          auto elapsed =
            (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
          if (!is_future_goal_handle_complete(elapsed)) {
            if (elapsed < server_timeout_) {
              return BT::NodeStatus::RUNNING;
            }
            handle_goal_response_timeout();
            return BT::NodeStatus::FAILURE;
          }
        }
 
        callback_group_executor_.spin_some();
 
        // check if, after invoking spin_some(), we finally received the result
        if (!goal_result_available_) {
          // Yield this Action, returning RUNNING
          return BT::NodeStatus::RUNNING;
        }
      }
    } catch (const std::runtime_error & e) {
      if (e.what() == std::string("Goal was rejected by the action server")) {
        on_goal_rejected();
        return BT::NodeStatus::FAILURE;
      } else if (e.what() == std::string("send_goal failed")) {
        on_send_goal_failure();
        // Action related failure that should not fail the tree, but the node
        return BT::NodeStatus::FAILURE;
      } else {
        // Internal exception to propagate to the tree
        throw e;
      }
    }
 
    BT::NodeStatus status;
    switch (result_.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        status = on_success();
        break;
 
      case rclcpp_action::ResultCode::ABORTED:
        status = on_aborted();
        break;
 
      case rclcpp_action::ResultCode::CANCELED:
        status = on_cancelled();
        break;
 
      default:
        throw std::logic_error("BtActionNode::Tick: invalid status value");
    }
 
    goal_handle_.reset();
    return status;
  }
 
  void halt() override
  {
    // Resolve a pending goal response before halting so an accepted goal can be
    // cancelled instead of being orphaned, but only within the goal's remaining
    // server_timeout_ budget.
    if (future_goal_handle_) {
      auto elapsed =
        (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
      auto remaining = server_timeout_ - elapsed;
      if (remaining > std::chrono::milliseconds(0)) {
        if (
          callback_group_executor_.spin_until_future_complete(
            *future_goal_handle_, remaining) ==
          rclcpp::FutureReturnCode::SUCCESS)
        {
          goal_handle_ = future_goal_handle_->get();
        }
      }
 
      future_goal_handle_.reset();
    }
 
    if (should_cancel_goal()) {
      auto future_result = action_client_->async_get_result(goal_handle_);
      auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
      if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
        rclcpp::FutureReturnCode::SUCCESS)
      {
        RCLCPP_ERROR(
          node_->get_logger(),
          "Failed to cancel action server for %s", action_name_.c_str());
      }
 
      if (callback_group_executor_.spin_until_future_complete(future_result, cancel_timeout_) !=
        rclcpp::FutureReturnCode::SUCCESS)
      {
        RCLCPP_ERROR(
          node_->get_logger(),
          "Failed to get result for %s in node halt!", action_name_.c_str());
      }
 
      on_cancelled();
    }
 
    // this is probably redundant, since the parent node
    // is supposed to call it, but we keep it, just in case
    resetStatus();
  }
 
protected:
  void handle_goal_response_timeout()
  {
    RCLCPP_WARN(
      node_->get_logger(),
      "Timed out waiting for action server to acknowledge goal request for %s, "
      "canceling all goals",
      action_name_.c_str());
    auto future_cancel = action_client_->async_cancel_all_goals();
    if (callback_group_executor_.spin_until_future_complete(
        future_cancel, cancel_timeout_) != rclcpp::FutureReturnCode::SUCCESS)
    {
      RCLCPP_WARN(
        node_->get_logger(),
        "Timed out while waiting for action server to cancel all goals for %s",
        action_name_.c_str());
    }
    future_goal_handle_.reset();
    on_timeout();
  }
 
  bool should_cancel_goal()
  {
    // Shut the node down if it is currently running
    if (status() != BT::NodeStatus::RUNNING) {
      return false;
    }
 
    // No need to cancel the goal if goal handle is invalid
    if (!goal_handle_) {
      return false;
    }
 
    callback_group_executor_.spin_some();
    auto status = goal_handle_->get_status();
 
    // Check if the goal is still executing
    return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
           status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
  }
 
  void send_new_goal()
  {
    goal_result_available_ = false;
    auto send_goal_options = typename nav2::ActionClient<ActionT>::SendGoalOptions();
    send_goal_options.result_callback =
      [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
        if (future_goal_handle_) {
          RCLCPP_DEBUG(
            node_->get_logger(),
            "Goal result for %s available, but it hasn't received the goal response yet. "
            "It's probably a goal result for the last goal request", action_name_.c_str());
          return;
        }
 
        // TODO(#1652): a work around until rcl_action interface is updated
        // if goal ids are not matched, the older goal call this callback so ignore the result
        // if matched, it must be processed (including aborted)
        if (this->goal_handle_->get_goal_id() == result.goal_id) {
          goal_result_available_ = true;
          result_ = result;
          emitWakeUpSignal();
        }
      };
    send_goal_options.feedback_callback =
      [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
      const std::shared_ptr<const typename ActionT::Feedback> feedback) {
        feedback_ = feedback;
        emitWakeUpSignal();
      };
 
    future_goal_handle_ = std::make_shared<
      std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
      action_client_->async_send_goal(goal_, send_goal_options));
    time_goal_sent_ = node_->now();
  }
 
  bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
  {
    auto remaining = server_timeout_ - elapsed;
 
    // server has already timed out, no need to sleep
    if (remaining <= std::chrono::milliseconds(0)) {
      return false;
    }
 
    auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
    auto result =
      callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
    elapsed += timeout;
 
    if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
      future_goal_handle_.reset();
      throw std::runtime_error("send_goal failed");
    }
 
    if (result == rclcpp::FutureReturnCode::SUCCESS) {
      goal_handle_ = future_goal_handle_->get();
      future_goal_handle_.reset();
      if (!goal_handle_) {
        throw std::runtime_error("Goal was rejected by the action server");
      }
      return true;
    }
 
    return false;
  }
 
  void increment_recovery_count()
  {
    int recovery_count = 0;
    [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count);  // NOLINT
    recovery_count += 1;
    config().blackboard->set("number_recoveries", recovery_count);  // NOLINT
  }
 
  std::string action_name_;
  typename nav2::ActionClient<ActionT>::SharedPtr action_client_;
 
  // All ROS2 actions have a goal and a result
  typename ActionT::Goal goal_;
  bool goal_updated_{false};
  bool goal_result_available_{false};
  typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
  typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
 
  // To handle feedback from action server
  std::shared_ptr<const typename ActionT::Feedback> feedback_;
 
  // The node that will be used for any ROS operations
  nav2::LifecycleNode::SharedPtr node_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
  // The timeout value while waiting for response from a server when a
  // new action goal is sent or canceled
  std::chrono::milliseconds server_timeout_;
 
  // The timeout value when cancelling actions
  std::chrono::milliseconds cancel_timeout_;
 
  // The timeout value for BT loop execution
  std::chrono::milliseconds max_timeout_;
 
  // The timeout value for waiting for a service to response
  std::chrono::milliseconds wait_for_service_timeout_;
 
  // To track the action server acknowledgement when a new goal is sent
  std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
  future_goal_handle_;
  rclcpp::Time time_goal_sent_;
 
  // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
  bool should_send_goal_;
};
 
}  // namespace nav2_behavior_tree
 
#endif  // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_