Program Listing for File behavior_tree_navigator.hpp
↰ Return to documentation for file (include/nav2_core/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.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/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(
rclcpp_lifecycle::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(
rclcpp_lifecycle::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);
// Create the Behavior Tree Action Server for this navigator
bt_action_server_ = std::make_unique<nav2_behavior_tree::BtActionServer<ActionT>>(
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));
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(rclcpp_lifecycle::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(
rclcpp_lifecycle::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>> 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_