00001 #include "crossdoor_nodes.h" 00002 00003 #include "behaviortree_cpp/loggers/bt_cout_logger.h" 00004 #include "behaviortree_cpp/loggers/bt_minitrace_logger.h" 00005 #include "behaviortree_cpp/loggers/bt_file_logger.h" 00006 00007 #include "behaviortree_cpp/bt_factory.h" 00008 00009 #ifdef ZMQ_FOUND 00010 #include "behaviortree_cpp/loggers/bt_zmq_publisher.h" 00011 #endif 00012 00023 // clang-format off 00024 00025 static const char* xml_text = R"( 00026 <root main_tree_to_execute = "MainTree"> 00027 <!---------------------------------------> 00028 <BehaviorTree ID="DoorClosed"> 00029 <Sequence name="door_closed_sequence"> 00030 <Inverter> 00031 <Condition ID="IsDoorOpen"/> 00032 </Inverter> 00033 <RetryUntilSuccesful num_attempts="4"> 00034 <OpenDoor/> 00035 </RetryUntilSuccesful> 00036 <PassThroughDoor/> 00037 </Sequence> 00038 </BehaviorTree> 00039 <!---------------------------------------> 00040 <BehaviorTree ID="MainTree"> 00041 <Sequence> 00042 <Fallback name="root_Fallback"> 00043 <Sequence name="door_open_sequence"> 00044 <IsDoorOpen/> 00045 <PassThroughDoor/> 00046 </Sequence> 00047 <SubTree ID="DoorClosed"/> 00048 <PassThroughWindow/> 00049 </Fallback> 00050 <CloseDoor/> 00051 </Sequence> 00052 </BehaviorTree> 00053 <!---------------------------------------> 00054 </root> 00055 )"; 00056 00057 // clang-format on 00058 00059 using namespace BT; 00060 00061 int main(int argc, char** argv) 00062 { 00063 BT::BehaviorTreeFactory factory; 00064 00065 // register all the actions into the factory 00066 CrossDoor::RegisterNodes(factory); 00067 00068 // Important: when the object tree goes out of scope, all the TreeNodes are destroyed 00069 auto tree = factory.createTreeFromText(xml_text); 00070 00071 // Create some loggers 00072 StdCoutLogger logger_cout(tree); 00073 MinitraceLogger logger_minitrace(tree, "bt_trace.json"); 00074 FileLogger logger_file(tree, "bt_trace.fbl"); 00075 #ifdef ZMQ_FOUND 00076 PublisherZMQ publisher_zmq(tree); 00077 #endif 00078 00079 printTreeRecursively(tree.root_node); 00080 00081 const bool LOOP = ( argc == 2 && strcmp( argv[1], "loop") == 0); 00082 00083 do 00084 { 00085 NodeStatus status = NodeStatus::RUNNING; 00086 // Keep on ticking until you get either a SUCCESS or FAILURE state 00087 while( status == NodeStatus::RUNNING) 00088 { 00089 status = tree.root_node->executeTick(); 00090 CrossDoor::SleepMS(1); // optional sleep to avoid "busy loops" 00091 } 00092 CrossDoor::SleepMS(1000); 00093 } 00094 while(LOOP); 00095 00096 return 0; 00097 }