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 "nav2_util/odometry_utils.hpp"
#include "tf2_ros/buffer.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "nav2_behavior_tree/bt_action_server.hpp"
 
namespace nav2_core
{
 
struct FeedbackUtils
{
  std::string robot_frame;
  std::string global_frame;
  double transform_tolerance;
  std::shared_ptr<tf2_ros::Buffer> tf;
};
 
class NavigatorMuxer
{
public:
  NavigatorMuxer()
  : current_navigator_(std::string("")) {}
 
  bool isNavigating()
  {
    std::scoped_lock l(mutex_);
    return !current_navigator_.empty();
  }
 
  void startNavigating(const std::string & navigator_name)
  {
    std::scoped_lock l(mutex_);
    if (!current_navigator_.empty()) {
      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_name;
  }
 
  void stopNavigating(const std::string & navigator_name)
  {
    std::scoped_lock l(mutex_);
    if (current_navigator_ != navigator_name) {
      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_ = std::string("");
    }
  }
 
protected:
  std::string current_navigator_;
  std::mutex mutex_;
};
 
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;
};
 
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>{ament_index_cpp::get_package_share_directory(
      "nav2_bt_navigator") + "/behavior_trees"}
    );
 
    // 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;
 
protected:
  bool onGoalReceived(typename ActionT::Goal::ConstSharedPtr goal)
  {
    if (plugin_muxer_->isNavigating()) {
      RCLCPP_ERROR(
        logger_,
        "Requested navigation from %s while another navigator is processing,"
        " rejecting request.", getName().c_str());
      return false;
    }
 
    bool goal_accepted = goalReceived(goal);
 
    if (goal_accepted) {
      plugin_muxer_->startNavigating(getName());
    }
 
    return goal_accepted;
  }
 
  void onCompletion(
    typename ActionT::Result::SharedPtr result,
    const nav2_behavior_tree::BtStatus final_bt_status)
  {
    plugin_muxer_->stopNavigating(getName());
    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,
    const 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_;
};
 
}  // namespace nav2_core
 
#endif  // NAV2_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_