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_