00001 /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 00002 * 00003 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 00004 * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 00005 * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 00006 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 00007 * 00008 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00009 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 00010 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 00011 */ 00012 00013 00014 #include<behavior_tree.h> 00015 #include <dot_bt.h> 00016 #include <ros/ros.h> 00017 00018 00019 00020 void Execute(BT::ControlNode* root, int TickPeriod_milliseconds) 00021 { 00022 std::cout << "Start Drawing!" << std::endl; 00023 // Starts in another thread the drawing of the BT 00024 std::thread t2(&drawTree, root); 00025 t2.detach(); 00026 BT::DotBt dotbt(root); 00027 std::thread t(&BT::DotBt::publish, dotbt); 00028 00029 root->ResetColorState(); 00030 00031 while (ros::ok()) 00032 { 00033 DEBUG_STDOUT("Ticking the root node !"); 00034 00035 // Ticking the root node 00036 root->Tick(); 00037 // Printing its state 00038 // root->GetNodeState(); 00039 00040 if (root->get_status() != BT::RUNNING) 00041 { 00042 // when the root returns a status it resets the colors of the tree 00043 root->ResetColorState(); 00044 } 00045 std::this_thread::sleep_for(std::chrono::milliseconds(TickPeriod_milliseconds)); 00046 } 00047 }