.. _program_listing_file__tmp_ws_src_ros2_planning_system_plansys2_executor_include_plansys2_executor_bt_builder_plugins_simple_bt_builder.hpp:

Program Listing for File simple_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_simple_bt_builder.hpp>` (``/tmp/ws/src/ros2_planning_system/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_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__SIMPLE_BT_BUILDER_HPP_
   #define PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__SIMPLE_BT_BUILDER_HPP_
   
   #include <string>
   #include <memory>
   #include <vector>
   #include <set>
   #include <list>
   #include <map>
   #include <utility>
   #include <tuple>
   
   #include "std_msgs/msg/empty.hpp"
   
   #include "plansys2_domain_expert/DomainExpertClient.hpp"
   #include "plansys2_problem_expert/ProblemExpertClient.hpp"
   #include "plansys2_executor/ActionExecutor.hpp"
   #include "plansys2_executor/BTBuilder.hpp"
   #include "plansys2_core/Types.hpp"
   #include "plansys2_msgs/msg/durative_action.hpp"
   #include "plansys2_msgs/msg/plan.hpp"
   
   #include "rclcpp/rclcpp.hpp"
   #include "rclcpp_action/rclcpp_action.hpp"
   
   namespace plansys2
   {
   
   struct GraphNode
   {
     using Ptr = std::shared_ptr<GraphNode>;
     static Ptr make_shared() {return std::make_shared<GraphNode>();}
   
     ActionStamped action;
     int node_num;
     int level_num;
   
     std::vector<plansys2::Predicate> predicates;
     std::vector<plansys2::Function> functions;
   
     std::list<GraphNode::Ptr> in_arcs;
     std::list<GraphNode::Ptr> out_arcs;
   };
   
   struct Graph
   {
     using Ptr = std::shared_ptr<Graph>;
     static Ptr make_shared() {return std::make_shared<Graph>();}
   
     std::list<GraphNode::Ptr> roots;
     std::map<float, std::list<GraphNode::Ptr>> levels;
   };
   
   class SimpleBTBuilder : public BTBuilder
   {
   public:
     SimpleBTBuilder();
     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:
     std::shared_ptr<plansys2::DomainExpertClient> domain_client_;
     std::shared_ptr<plansys2::ProblemExpertClient> problem_client_;
   
     Graph::Ptr graph_;
     std::string bt_;
     std::string bt_action_;
   
     Graph::Ptr get_graph(const plansys2_msgs::msg::Plan & current_plan);
   
     std::vector<ActionStamped> get_plan_actions(const plansys2_msgs::msg::Plan & plan);
     void prune_backwards(GraphNode::Ptr new_node, GraphNode::Ptr node_satisfy);
     void prune_forward(GraphNode::Ptr current, std::list<GraphNode::Ptr> & used_nodes);
     void get_state(
       const GraphNode::Ptr & node,
       std::list<GraphNode::Ptr> & used_nodes,
       std::vector<plansys2::Predicate> & predicates,
       std::vector<plansys2::Function> & functions) const;
   
     bool is_action_executable(
       const ActionStamped & action,
       std::vector<plansys2::Predicate> & predicates,
       std::vector<plansys2::Function> & functions) const;
     std::list<GraphNode::Ptr> get_roots(
       std::vector<plansys2::ActionStamped> & action_sequence,
       std::vector<plansys2::Predicate> & predicates,
       std::vector<plansys2::Function> & functions,
       int & node_counter);
     GraphNode::Ptr get_node_satisfy(
       const plansys2_msgs::msg::Tree & requirement,
       const Graph::Ptr & graph,
       const GraphNode::Ptr & current);
     GraphNode::Ptr get_node_satisfy(
       const plansys2_msgs::msg::Tree & requirement,
       const GraphNode::Ptr & node,
       const GraphNode::Ptr & current);
     std::list<GraphNode::Ptr> get_node_contradict(
       const Graph::Ptr & graph,
       const GraphNode::Ptr & current);
     void get_node_contradict(
       const GraphNode::Ptr & node,
       const GraphNode::Ptr & current,
       std::list<GraphNode::Ptr> & parents);
     void remove_existing_requirements(
       std::vector<plansys2_msgs::msg::Tree> & requirements,
       std::vector<plansys2::Predicate> & predicates,
       std::vector<plansys2::Function> & functions) const;
     bool is_parallelizable(
       const plansys2::ActionStamped & action,
       const std::vector<plansys2::Predicate> & predicates,
       const std::vector<plansys2::Function> & functions,
       const std::list<GraphNode::Ptr> & ret) const;
   
     std::string get_flow_tree(
       GraphNode::Ptr node,
       std::list<std::string> & used_nodes,
       int level = 0);
     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, int level = 0);
     ActionExecutor::Status get_action_status(
       ActionStamped action,
       std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map);
     void addDotGraphLegend(
       std::stringstream & ss, int tab_level, int level_counter,
       int node_counter);
   
     std::string t(int level);
   
     std::string execution_block(const GraphNode::Ptr & node, int l);
     void print_node(
       const GraphNode::Ptr & node,
       int level,
       std::set<GraphNode::Ptr> & used_nodes) const;
   
     void print_graph(const plansys2::Graph::Ptr & graph) const;
   
     void print_node_csv(const GraphNode::Ptr & node, uint32_t root_num) const;
     void print_graph_csv(const plansys2::Graph::Ptr & graph) const;
   
     void get_node_tabular(
       const plansys2::GraphNode::Ptr & node,
       uint32_t root_num,
       std::vector<std::tuple<uint32_t, uint32_t, uint32_t, std::string>> & graph) const;
     std::vector<std::tuple<uint32_t, uint32_t, uint32_t, std::string>> get_graph_tabular(
       const plansys2::Graph::Ptr & graph) const;
   };
   
   }  // namespace plansys2
   
   
   #include "pluginlib/class_list_macros.hpp"
   
   PLUGINLIB_EXPORT_CLASS(plansys2::SimpleBTBuilder, plansys2::BTBuilder)
   
   #endif  // PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__SIMPLE_BT_BUILDER_HPP_