Program Listing for File bt_action_node.hpp

↰ Return to documentation for file (include/as2_behavior_tree/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 AS2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#define AS2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_

#include <chrono>
#include <memory>
#include <string>
#include <set>

#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_behavior_tree/bt_conversions.hpp"
#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

namespace nav2_behavior_tree
{

using namespace std::chrono_literals; // NOLINT

template<class ActionT>
class BtActionNode : public BT::ActionNodeBase
{
public:
  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)
  {
    node_ = config().blackboard->template get<rclcpp::Node::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
    bt_loop_duration_ =
      config().blackboard->template get<std::chrono::milliseconds>(
      "bt_loop_duration");
    server_timeout_ =
      config().blackboard->template get<std::chrono::milliseconds>(
      "server_timeout");
    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);

    // Initialize the input and output messages
    goal_ = typename ActionT::Goal();
    result_ =
      typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();

    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_ = rclcpp_action::create_client<ActionT>(
      node_, 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(1s)) {
      RCLCPP_ERROR(
        node_->get_logger(),
        "\"%s\" action server not available after waiting for 1 s",
        action_name.c_str());
      throw std::runtime_error(
              std::string("Action server %s not available", action_name.c_str()));
    }
  }

  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")};
    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;}

  BT::NodeStatus tick() override
  {
    // first step to be done only at the beginning of the Action
    if (status() == BT::NodeStatus::IDLE) {
      // setting the status to RUNNING to notify the BT Loggers (if any)
      setStatus(BT::NodeStatus::RUNNING);

      // user defined callback
      on_tick();

      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_)
          .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;
          }
          // if server has taken more time than the specified timeout value
          // return FAILURE
          RCLCPP_WARN(
            node_->get_logger(),
            "Timed out while waiting for action server to "
            "acknowledge goal request for %s",
            action_name_.c_str());
          future_goal_handle_.reset();
          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_)
            .to_chrono<std::chrono::milliseconds>();
          if (!is_future_goal_handle_complete(elapsed)) {
            if (elapsed < server_timeout_) {
              return BT::NodeStatus::RUNNING;
            }
            RCLCPP_WARN(
              node_->get_logger(),
              "Timed out while waiting for action server to "
              "acknowledge goal request for %s",
              action_name_.c_str());
            future_goal_handle_.reset();
            return BT::NodeStatus::FAILURE;
          }
        }

        auto ros_start = node_->now();
        auto start = std::chrono::system_clock::now();
        callback_group_executor_.spin_some();
        auto ros_end = node_->now();
        auto end = std::chrono::system_clock::now();

        std::chrono::duration<double> diff = end - start;
        // std::cout << "SPIN SOME: Time " << std::setw(9) << " : " <<
        // diff.count() << " s\n";

        rclcpp::Duration ros_diff = ros_end - ros_start;
        // std::cout << "SPIN SOME: ROS Time " << std::setw(9) << " : " <<
        // ros_diff.nanoseconds() << " ns\n";

        // 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("send_goal failed") ||
        e.what() == std::string("Goal was rejected by the action server"))
      {
        // Action related failure that should not fail the tree, but the node
        return BT::NodeStatus::FAILURE;
      } else {
        // Internal exception to propogate 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
  {
    if (should_cancel_goal()) {
      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());
      }
    }

    setStatus(BT::NodeStatus::IDLE);
  }

protected:
  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 rclcpp_action::Client<ActionT>::SendGoalOptions();

    // send_goal_options.goal_response_callback =
    //     [this](std::shared_future<typename
    //     rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr> future)
    // {
    //     auto goal_handle = future.get();
    //     if (!goal_handle) {
    //         RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
    //     } else {
    //         RCLCPP_INFO(node_->get_logger(), "Goal accepted by server,
    //         waiting for result");
    //     }
    // };

    send_goal_options.result_callback =
      std::bind(&BtActionNode::result_callback, this, std::placeholders::_1);

    // send_goal_options.result_callback =
    //     [this](const typename
    //     rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult &result)
    // {
    //     RCLCPP_INFO(node_->get_logger(), "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;
    //     }
    // };
    send_goal_options.feedback_callback =
      [this](
      typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
      const std::shared_ptr<const typename ActionT::Feedback> feedback) {
        feedback_ = feedback;
      };

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

  void result_callback(
    const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult
    & result)
  {
    RCLCPP_INFO(node_->get_logger(), "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;
    }
  }

  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)) {
      future_goal_handle_.reset();
      return false;
    }

    auto timeout =
      remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
    auto ros_start = node_->now().nanoseconds();
    auto start = std::chrono::system_clock::now();
    auto result = callback_group_executor_.spin_until_future_complete(
      *future_goal_handle_, timeout);
    auto ros_end = node_->now().nanoseconds();
    auto end = std::chrono::system_clock::now();

    std::chrono::duration<double> diff = end - start;
    // std::cout << "Time " << std::setw(9) << " : " << diff.count() << " s\n";

    auto ros_diff = ros_end - ros_start;
    // std::cout << "ROS Time " << std::setw(9) << " : " << ros_diff << " ns\n";

    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");
      }
      // TODO(pariaspe): be careful, result will be always successful
      // goal_result_available_ = true;
      // result_.code = rclcpp_action::ResultCode::SUCCEEDED;
      return true;
    }

    return false;
  }

  void increment_recovery_count()
  {
    int recovery_count = 0;
    config().blackboard->template get<int>(
      "number_recoveries",
      recovery_count);                                      // NOLINT
    recovery_count += 1;
    config().blackboard->template set<int>(
      "number_recoveries",
      recovery_count);                                      // NOLINT
  }

  std::string action_name_;
  typename std::shared_ptr<rclcpp_action::Client<ActionT>> 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
  rclcpp::Node::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 for BT loop execution
  std::chrono::milliseconds bt_loop_duration_;

  // 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_;
};

}  // namespace nav2_behavior_tree

#endif  // AS2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_