.. _program_listing_file__tmp_ws_src_ros2_planning_system_plansys2_executor_include_plansys2_executor_bt_builder_plugins_stn_bt_builder.hpp:

Program Listing for File stn_bt_builder.hpp
===========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ros2_planning_system_plansys2_executor_include_plansys2_executor_bt_builder_plugins_stn_bt_builder.hpp>` (``/tmp/ws/src/ros2_planning_system/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright 2020 Intelligent Robotics Lab
   //
   // 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 PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__STN_BT_BUILDER_HPP_
   #define PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__STN_BT_BUILDER_HPP_
   
   #include <list>
   #include <map>
   #include <memory>
   #include <set>
   #include <string>
   #include <tuple>
   #include <utility>
   #include <vector>
   
   #include "plansys2_domain_expert/DomainExpertClient.hpp"
   #include "plansys2_executor/ActionExecutor.hpp"
   #include "plansys2_executor/BTBuilder.hpp"
   #include "plansys2_msgs/msg/plan.hpp"
   #include "plansys2_problem_expert/ProblemExpertClient.hpp"
   
   namespace plansys2
   {
   
   struct StateVec
   {
     std::vector<plansys2::Predicate> predicates;
     std::vector<plansys2::Function> functions;
   };
   
   struct GraphNode
   {
     using Ptr = std::shared_ptr<GraphNode>;
     static Ptr make_shared(int id) {return std::make_shared<GraphNode>(id);}
   
     int node_num;
     ActionStamped action;
   
     std::set<std::tuple<GraphNode::Ptr, double, double>> input_arcs;
     std::set<std::tuple<GraphNode::Ptr, double, double>> output_arcs;
   
     explicit GraphNode(int id)
     : node_num(id) {}
   };
   
   struct Graph
   {
     using Ptr = std::shared_ptr<Graph>;
     static Ptr make_shared() {return std::make_shared<Graph>();}
   
     std::list<GraphNode::Ptr> nodes;
   };
   
   class STNBTBuilder : public BTBuilder
   {
   public:
     STNBTBuilder();
     void initialize(
       const std::string & bt_action_1 = "",
       const std::string & bt_action_2 = "",
       int precision = 3);
   
     std::string get_tree(const plansys2_msgs::msg::Plan & current_plan);
     std::string get_dotgraph(
       std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map,
       bool enable_legend = false,
       bool enable_print_graph = false);
   
   protected:
     Graph::Ptr build_stn(const plansys2_msgs::msg::Plan & plan) const;
     std::string build_bt(const Graph::Ptr stn) const;
   
     Graph::Ptr init_graph(const plansys2_msgs::msg::Plan & plan) const;
     std::vector<ActionStamped> get_plan_actions(const plansys2_msgs::msg::Plan & plan) const;
   
     std::set<int> get_happenings(const plansys2_msgs::msg::Plan & plan) const;
     std::set<int>::iterator get_happening(int time, const std::set<int> & happenings) const;
     std::set<int>::iterator get_previous(int time, const std::set<int> & happenings) const;
   
     std::multimap<int, ActionStamped> get_simple_plan(const plansys2_msgs::msg::Plan & plan) const;
   
     std::map<int, StateVec> get_states(
       const std::set<int> & happenings,
       const std::multimap<int, ActionStamped> & plan) const;
     plansys2_msgs::msg::Tree from_state(
       const std::vector<plansys2::Predicate> & preds,
       const std::vector<plansys2::Function> & funcs) const;
   
     std::vector<GraphNode::Ptr> get_nodes(
       const ActionStamped & action,
       const Graph::Ptr graph) const;
   
     bool is_match(
       const GraphNode::Ptr node,
       const ActionStamped & action) const;
   
     std::vector<std::pair<int, ActionStamped>> get_parents(
       const std::pair<int, ActionStamped> & action,
       const std::multimap<int, ActionStamped> & plan,
       const std::set<int> & happenings,
       const std::map<int, StateVec> & states) const;
   
     std::vector<std::pair<int, ActionStamped>> get_satisfy(
       const std::pair<int, ActionStamped> & action,
       const std::multimap<int, ActionStamped> & plan,
       const std::set<int> & happenings,
       const std::map<int, StateVec> & states) const;
   
     std::vector<std::pair<int, ActionStamped>> get_threat(
       const std::pair<int, ActionStamped> & action,
       const std::multimap<int, ActionStamped> & plan,
       const std::set<int> & happenings,
       const std::map<int, StateVec> & states) const;
   
     bool can_apply(
       const std::pair<int, ActionStamped> & action,
       const std::multimap<int, ActionStamped> & plan,
       const int & time,
       StateVec & state) const;
   
     StateVec get_diff(const StateVec & X_1, const StateVec & X_2) const;
     StateVec get_intersection(const StateVec & X_1, const StateVec & X_2) const;
   
     plansys2_msgs::msg::Tree get_conditions(const ActionStamped & action) const;
     plansys2_msgs::msg::Tree get_effects(const ActionStamped & action) const;
   
     void prune_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const;
     bool check_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const;
   
     std::string get_flow(
       const GraphNode::Ptr node,
       const GraphNode::Ptr parent,
       std::set<GraphNode::Ptr> & used,
       const int & level) const;
   
     std::string start_execution_block(
       const GraphNode::Ptr node,
       const GraphNode::Ptr parent,
       const int & l) const;
     std::string end_execution_block(
       const GraphNode::Ptr node,
       const GraphNode::Ptr parent,
       const int & l) const;
   
     void get_flow_dotgraph(
       GraphNode::Ptr node,
       std::set<std::string> & edges);
     std::string get_node_dotgraph(
       GraphNode::Ptr node,
       std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map);
     ActionExecutor::Status get_action_status(
       ActionStamped action,
       std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map);
     std::string add_dot_graph_legend(
       int level_counter,
       int node_counter);
     void print_graph(const plansys2::Graph::Ptr graph) const;
     void print_node(const GraphNode::Ptr node, int level) const;
   
     void replace(std::string & str, const std::string & from, const std::string & to) const;
   
     bool is_end(
       const std::tuple<GraphNode::Ptr, double, double> & edge,
       const ActionStamped & action) const;
   
     std::string t(const int & level) const;
   
     std::shared_ptr<plansys2::DomainExpertClient> domain_client_;
     std::shared_ptr<plansys2::ProblemExpertClient> problem_client_;
   
     Graph::Ptr stn_;
     std::string bt_start_action_;
     std::string bt_end_action_;
     int action_time_precision_;
   };
   
   }  // namespace plansys2
   
   
   #include "pluginlib/class_list_macros.hpp"
   
   PLUGINLIB_EXPORT_CLASS(plansys2::STNBTBuilder, plansys2::BTBuilder)
   
   #endif  // PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__STN_BT_BUILDER_HPP_