bt_cancel_action_node.hpp

// Copyright (c) 2022 Neobotix GmbH
//
// 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_CANCEL_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
 
#include <memory>
#include <string>
#include <chrono>
 
#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
 
namespace nav2_behavior_tree
{
 
using namespace std::chrono_literals;  // NOLINT
 
template<class ActionT>
class BtCancelActionNode : public BT::ActionNodeBase
{
public:
  BtCancelActionNode(
    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
    server_timeout_ =
      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
    wait_for_service_timeout_ =
      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
    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\" BtCancelActionNode initialized",
      xml_tag_name.c_str());
  }
 
  BtCancelActionNode() = delete;
 
  virtual ~BtCancelActionNode()
  {
  }
 
  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(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")
    };
    basic.insert(addition.begin(), addition.end());
 
    return basic;
  }
 
  void halt() override
  {
  }
 
  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({});
  }
 
  BT::NodeStatus tick() override
  {
    // setting the status to RUNNING to notify the BT Loggers (if any)
    setStatus(BT::NodeStatus::RUNNING);
 
    // Cancel all the goals specified before 10ms from current time
    // to avoid async communication error
 
    rclcpp::Time goal_expiry_time = node_->now() - std::chrono::milliseconds(10);
 
    auto future_cancel = action_client_->async_cancel_goals_before(goal_expiry_time);
 
    if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
      rclcpp::FutureReturnCode::SUCCESS)
    {
      RCLCPP_ERROR(
        node_->get_logger(),
        "Failed to cancel the action server for %s", action_name_.c_str());
      return BT::NodeStatus::FAILURE;
    }
    return BT::NodeStatus::SUCCESS;
  }
 
protected:
  std::string action_name_;
  typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
 
  // 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 canceled
  std::chrono::milliseconds server_timeout_;
  // The timeout value for waiting for a service to response
  std::chrono::milliseconds wait_for_service_timeout_;
};
 
}  // namespace nav2_behavior_tree
 
#endif  // NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_