behavior_tree_navigator.hpp

// Copyright (c) 2021-2023 Samsung Research America
//
// 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_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_
#define NAV2_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_
 
#include <memory>
#include <string>
#include <vector>
#include <mutex>
#include <chrono>
 
#include "nav2_util/odometry_utils.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/rate.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "nav2_behavior_tree/bt_action_server.hpp"
#include "nav2_ros_common/node_utils.hpp"
 
namespace nav2_core
{
class NavigatorMuxer;
 
struct FeedbackUtils
{
  std::string robot_frame;
  std::string global_frame;
  double transform_tolerance;
  nav2::TransformBuffer::SharedPtr tf;
};
 
class NavigatorBase
{
public:
  NavigatorBase() = default;
  virtual ~NavigatorBase() = default;
 
  virtual bool on_configure(
    nav2::LifecycleNode::WeakPtr parent_node,
    const std::vector<std::string> & plugin_lib_names,
    const FeedbackUtils & feedback_utils,
    nav2_core::NavigatorMuxer * plugin_muxer,
    std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) = 0;
 
  virtual bool on_activate() = 0;
 
  virtual bool on_deactivate() = 0;
 
  virtual bool on_cleanup() = 0;
 
  virtual void preempt() = 0;
};
 
class NavigatorMuxer
{
public:
  NavigatorMuxer()
  : current_navigator_(nullptr) {}
 
  bool isNavigating()
  {
    std::scoped_lock l(mutex_);
    return current_navigator_ != nullptr;
  }
 
  void startNavigating(nav2_core::NavigatorBase * navigator)
  {
    std::scoped_lock l(mutex_);
    if (current_navigator_ != nullptr) {
      RCLCPP_ERROR(
        rclcpp::get_logger("NavigatorMutex"),
        "Major error! Navigation requested while another navigation"
        " task is in progress! This likely occurred from an incorrect"
        "implementation of a navigator plugin.");
    }
    current_navigator_ = navigator;
  }
 
  void stopNavigating(nav2_core::NavigatorBase * navigator)
  {
    std::scoped_lock l(mutex_);
    if (current_navigator_ != navigator) {
      RCLCPP_ERROR(
        rclcpp::get_logger("NavigatorMutex"),
        "Major error! Navigation stopped while another navigation"
        " task is in progress! This likely occurred from an incorrect"
        "implementation of a navigator plugin.");
    } else {
      current_navigator_ = nullptr;
    }
  }
 
  void preemptCurrentNavigator()
  {
    std::scoped_lock l(mutex_);
    current_navigator_->preempt();
  }
 
  bool isCurrentNavigator(nav2_core::NavigatorBase * navigator)
  {
    std::scoped_lock l(mutex_);
    return current_navigator_ == navigator;
  }
 
protected:
  nav2_core::NavigatorBase * current_navigator_{nullptr};
  std::mutex mutex_;
};
 
template<class ActionT>
class BehaviorTreeNavigator : public NavigatorBase
{
public:
  using Ptr = std::shared_ptr<nav2_core::BehaviorTreeNavigator<ActionT>>;
 
  BehaviorTreeNavigator()
  : NavigatorBase()
  {
    plugin_muxer_ = nullptr;
  }
 
  virtual ~BehaviorTreeNavigator() = default;
 
  bool on_configure(
    nav2::LifecycleNode::WeakPtr parent_node,
    const std::vector<std::string> & plugin_lib_names,
    const FeedbackUtils & feedback_utils,
    nav2_core::NavigatorMuxer * plugin_muxer,
    std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) final
  {
    auto node = parent_node.lock();
    logger_ = node->get_logger();
    clock_ = node->get_clock();
    feedback_utils_ = feedback_utils;
    plugin_muxer_ = plugin_muxer;
 
    // get the default behavior tree for this navigator
    std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node);
 
    auto search_directories = node->declare_or_get_parameter(
      "bt_search_directories",
      std::vector<std::string>{nav2::get_package_share_directory(
          "nav2_bt_navigator") + "/behavior_trees"}
    );
 
    allow_navigator_preemption_ = node->declare_or_get_parameter(
      "allow_navigator_preemption", false);
 
    navigator_preemption_timeout_ = std::chrono::milliseconds(
      node->declare_or_get_parameter("navigator_preemption_timeout", 500));
 
    // Create the Behavior Tree Action Server for this navigator
    bt_action_server_ =
      std::make_unique<nav2_behavior_tree::BtActionServer<ActionT, nav2::LifecycleNode>>(
      node,
      getName(),
      plugin_lib_names,
      default_bt_xml_filename,
      std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1),
      std::bind(&BehaviorTreeNavigator::onLoop, this),
      std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1),
      std::bind(
        &BehaviorTreeNavigator::onCompletion, this,
        std::placeholders::_1, std::placeholders::_2),
      search_directories);
 
    bool ok = true;
    if (!bt_action_server_->on_configure()) {
      ok = false;
    }
 
    BT::Blackboard::Ptr blackboard = bt_action_server_->getBlackboard();
    blackboard->set("tf_buffer", feedback_utils.tf);  // NOLINT
    blackboard->set("initial_pose_received", false);  // NOLINT
    blackboard->set("number_recoveries", 0);  // NOLINT
    blackboard->set("odom_smoother", odom_smoother);  // NOLINT
 
    return configure(parent_node, odom_smoother) && ok;
  }
 
  bool on_activate() final
  {
    bool ok = true;
 
    if (!bt_action_server_->on_activate()) {
      ok = false;
    }
 
    return activate() && ok;
  }
 
  bool on_deactivate() final
  {
    bool ok = true;
    if (!bt_action_server_->on_deactivate()) {
      ok = false;
    }
 
    return deactivate() && ok;
  }
 
  bool on_cleanup() final
  {
    bool ok = true;
    if (!bt_action_server_->on_cleanup()) {
      ok = false;
    }
 
    bt_action_server_.reset();
 
    return cleanup() && ok;
  }
 
  virtual std::string getDefaultBTFilepath(nav2::LifecycleNode::WeakPtr node) = 0;
 
  virtual std::string getName() = 0;
 
  void preempt() final
  {
    bt_action_server_->preemptCurrentNavigator();
  }
 
protected:
  bool onGoalReceived(typename ActionT::Goal::ConstSharedPtr goal)
  {
    if (plugin_muxer_->isNavigating() && !plugin_muxer_->isCurrentNavigator(this)) {
      // A different navigator is processing; check if cross-navigator preemption is allowed
      if (!allow_navigator_preemption_) {
        RCLCPP_ERROR(
          logger_,
          "Requested navigation from %s while another navigator is processing,"
          " rejecting request.", getName().c_str());
        return false;
      }
 
      RCLCPP_INFO(
        logger_,
        "Requested navigation from %s while another navigator is processing,"
        " stopping current navigator.", getName().c_str());
      plugin_muxer_->preemptCurrentNavigator();
 
      const auto start = std::chrono::steady_clock::now();
      rclcpp::Rate r(100);
      while (plugin_muxer_->isNavigating()) {
        if (std::chrono::steady_clock::now() - start > navigator_preemption_timeout_) {
          RCLCPP_ERROR(
            logger_,
            "Timed out waiting for current navigator to stop before accepting"
            " goal from %s. Rejecting request.", getName().c_str());
          return false;
        }
        r.sleep();
      }
    }
 
    bool goal_accepted = goalReceived(goal);
 
    if (goal_accepted) {
      plugin_muxer_->startNavigating(this);
    }
 
    return goal_accepted;
  }
 
  void onCompletion(
    typename ActionT::Result::SharedPtr result,
    nav2_behavior_tree::BtStatus & final_bt_status)
  {
    plugin_muxer_->stopNavigating(this);
    goalCompleted(result, final_bt_status);
  }
 
  virtual bool goalReceived(typename ActionT::Goal::ConstSharedPtr goal) = 0;
 
  virtual void onLoop() = 0;
 
  virtual void onPreempt(typename ActionT::Goal::ConstSharedPtr goal) = 0;
 
  virtual void goalCompleted(
    typename ActionT::Result::SharedPtr result,
    nav2_behavior_tree::BtStatus & final_bt_status) = 0;
 
  virtual bool configure(
    nav2::LifecycleNode::WeakPtr /*node*/,
    std::shared_ptr<nav2_util::OdomSmoother>/*odom_smoother*/)
  {
    return true;
  }
 
  virtual bool cleanup() {return true;}
 
  virtual bool activate() {return true;}
 
  virtual bool deactivate() {return true;}
 
  std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT, nav2::LifecycleNode>>
  bt_action_server_;
  rclcpp::Logger logger_{rclcpp::get_logger("Navigator")};
  rclcpp::Clock::SharedPtr clock_;
  FeedbackUtils feedback_utils_;
  NavigatorMuxer * plugin_muxer_;
 
  // True if you want to allow navigator preemption
  bool allow_navigator_preemption_;
 
  // Timeout value while waiting for preemption of a navigator by another one
  std::chrono::milliseconds navigator_preemption_timeout_;
};
 
}  // namespace nav2_core
 
#endif  // NAV2_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_