Program Listing for File bt_action_server.hpp
↰ Return to documentation for file (include/nav2_behavior_tree/bt_action_server.hpp
)
// Copyright (c) 2020 Sarthak Mittal
//
// 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_ACTION_SERVER_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
#include "nav2_behavior_tree/ros_topic_logger.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/simple_action_server.hpp"
namespace nav2_behavior_tree
{
template<class ActionT>
class BtActionServer
{
public:
using ActionServer = nav2_util::SimpleActionServer<ActionT>;
typedef std::function<bool (typename ActionT::Goal::ConstSharedPtr)> OnGoalReceivedCallback;
typedef std::function<void ()> OnLoopCallback;
typedef std::function<void (typename ActionT::Goal::ConstSharedPtr)> OnPreemptCallback;
typedef std::function<void (typename ActionT::Result::SharedPtr,
nav2_behavior_tree::BtStatus)> OnCompletionCallback;
explicit BtActionServer(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & action_name,
const std::vector<std::string> & plugin_lib_names,
const std::string & default_bt_xml_filename,
OnGoalReceivedCallback on_goal_received_callback,
OnLoopCallback on_loop_callback,
OnPreemptCallback on_preempt_callback,
OnCompletionCallback on_completion_callback);
~BtActionServer();
bool on_configure();
bool on_activate();
bool on_deactivate();
bool on_cleanup();
bool loadBehaviorTree(const std::string & bt_xml_filename = "");
BT::Blackboard::Ptr getBlackboard() const
{
return blackboard_;
}
std::string getCurrentBTFilename() const
{
return current_bt_xml_filename_;
}
std::string getDefaultBTFilename() const
{
return default_bt_xml_filename_;
}
const std::shared_ptr<const typename ActionT::Goal> acceptPendingGoal()
{
return action_server_->accept_pending_goal();
}
void terminatePendingGoal()
{
action_server_->terminate_pending_goal();
}
const std::shared_ptr<const typename ActionT::Goal> getCurrentGoal() const
{
return action_server_->get_current_goal();
}
const std::shared_ptr<const typename ActionT::Goal> getPendingGoal() const
{
return action_server_->get_pending_goal();
}
void publishFeedback(typename std::shared_ptr<typename ActionT::Feedback> feedback)
{
action_server_->publish_feedback(feedback);
}
const BT::Tree & getTree() const
{
return tree_;
}
void haltTree()
{
tree_.haltTree();
}
protected:
void executeCallback();
void populateErrorCode(typename std::shared_ptr<typename ActionT::Result> result);
void cleanErrorCodes();
// Action name
std::string action_name_;
// Our action server implements the template action
std::shared_ptr<ActionServer> action_server_;
// Behavior Tree to be executed when goal is received
BT::Tree tree_;
// The blackboard shared by all of the nodes in the tree
BT::Blackboard::Ptr blackboard_;
// The XML file that cointains the Behavior Tree to create
std::string current_bt_xml_filename_;
std::string default_bt_xml_filename_;
// The wrapper class for the BT functionality
std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
// Libraries to pull plugins (BT Nodes) from
std::vector<std::string> plugin_lib_names_;
// Error code id names
std::vector<std::string> error_code_names_;
// A regular, non-spinning ROS node that we can use for calls to the action client
rclcpp::Node::SharedPtr client_node_;
// Parent node
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
// Clock
rclcpp::Clock::SharedPtr clock_;
// Logger
rclcpp::Logger logger_{rclcpp::get_logger("BtActionServer")};
// To publish BT logs
std::unique_ptr<RosTopicLogger> topic_logger_;
// Duration for each iteration of BT execution
std::chrono::milliseconds bt_loop_duration_;
// Default timeout value while waiting for response from a server
std::chrono::milliseconds default_server_timeout_;
// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
// should the BT be reloaded even if the same xml filename is requested?
bool always_reload_bt_xml_ = false;
// User-provided callbacks
OnGoalReceivedCallback on_goal_received_callback_;
OnLoopCallback on_loop_callback_;
OnPreemptCallback on_preempt_callback_;
OnCompletionCallback on_completion_callback_;
};
} // namespace nav2_behavior_tree
#include <nav2_behavior_tree/bt_action_server_impl.hpp> // NOLINT(build/include_order)
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_