├── .gitignore ├── .travis.yml ├── BTppUserManual.pdf ├── CMakeLists.txt ├── LICENSE ├── README.md ├── conf ├── BTppConfig.cmake.in └── BTppConfigVersion.cmake.in ├── contributors.txt ├── gtest ├── gtest_tree.cpp ├── include │ ├── action_test_node.h │ └── condition_test_node.h └── src │ ├── action_test_node.cpp │ └── condition_test_node.cpp ├── include ├── action_node.h ├── behavior_tree.h ├── condition_node.h ├── control_node.h ├── decorator_negation_node.h ├── decorator_retry_node.h ├── draw.h ├── exceptions.h ├── fallback_node.h ├── fallback_node_with_memory.h ├── leaf_node.h ├── parallel_node.h ├── sequence_node.h ├── sequence_node_with_memory.h ├── tick_engine.h └── tree_node.h ├── src ├── action_node.cpp ├── behavior_tree.cpp ├── condition_node.cpp ├── control_node.cpp ├── decorator_negation_node.cpp ├── decorator_retry_node.cpp ├── draw.cpp ├── example.cpp ├── exceptions.cpp ├── fallback_node.cpp ├── fallback_node_with_memory.cpp ├── leaf_node.cpp ├── parallel_node.cpp ├── sequence_node.cpp ├── sequence_node_with_memory.cpp ├── tick_engine.cpp ├── tree.cpp └── tree_node.cpp └── templates ├── action_node_template.cpp └── condition_node_template.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: cpp 4 | 5 | os: 6 | - linux 7 | 8 | compiler: 9 | - gcc 10 | - clang 11 | 12 | before_install: 13 | - sudo apt-get update && sudo apt-get --reinstall install -qq build-essential freeglut3-dev libxmu-dev libxi-dev 14 | 15 | before_script: 16 | # Prepare build directory 17 | - mkdir -p build 18 | 19 | script: 20 | # Build BTpp 21 | - (cd build; cmake .. ;cmake --build . ) 22 | #Install BTpp 23 | - (cd build; sudo make install) 24 | 25 | -------------------------------------------------------------------------------- /BTppUserManual.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miccol/Behavior-Tree/5e25de7aa65ca947698f5506c4a56502472bc3de/BTppUserManual.pdf -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(BTpp) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | add_definitions(-lX11 -Wall -lglut -lGL -lgtest -std=c++11 -lrt ) 6 | # Needed for using threads 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") 8 | 9 | ######################################################### 10 | # FIND X11 11 | ######################################################### 12 | find_package(X11 REQUIRED) 13 | include_directories(${X11_INCLUDE_DIR}) 14 | link_directories(${X11_LIBRARIES}) 15 | 16 | ######################################################### 17 | # FIND GTest 18 | ######################################################### 19 | find_package(GTest) 20 | include_directories(${GTEST_INCLUDE_DIRS}) 21 | if(NOT GTEST_FOUND) 22 | message(WARINING " GTest not found!") 23 | endif(NOT GTEST_FOUND) 24 | 25 | ######################################################### 26 | # FIND GLUT 27 | ######################################################### 28 | find_package(GLUT REQUIRED) 29 | include_directories(${GLUT_INCLUDE_DIRS}) 30 | link_directories(${GLUT_LIBRARY_DIRS}) 31 | add_definitions(${GLUT_DEFINITIONS}) 32 | if(NOT GLUT_FOUND) 33 | message(ERROR " GLUT not found!") 34 | endif(NOT GLUT_FOUND) 35 | 36 | ######################################################### 37 | # FIND OPENGL 38 | ######################################################### 39 | find_package(OpenGL REQUIRED) 40 | include_directories(${OpenGL_INCLUDE_DIRS}) 41 | link_directories(${OpenGL_LIBRARY_DIRS}) 42 | add_definitions(${OpenGL_DEFINITIONS}) 43 | if(NOT OPENGL_FOUND) 44 | message(ERROR " OPENGL not found!") 45 | endif(NOT OPENGL_FOUND) 46 | 47 | 48 | INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/include/) 49 | INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/gtest/include/) 50 | 51 | if(APPLE) 52 | include_directories(AFTER "/opt/X11/include") 53 | set(CMAKE_OSX_ARCHITECTURES "x86_64") 54 | if(CMAKE_GENERATOR STREQUAL Xcode) 55 | set(CMAKE_OSX_DEPLOYMENT_TARGET "10.8") 56 | endif() 57 | endif() 58 | 59 | 60 | ######################################################### 61 | # Make relative paths absolute (needed later on) 62 | ######################################################### 63 | 64 | foreach(p LIB BIN INCLUDE CMAKE) 65 | set(var INSTALL_${p}_DIR) 66 | if(NOT IS_ABSOLUTE "${${var}}") 67 | set(${var} "${CMAKE_INSTALL_PREFIX}/${${var}}") 68 | endif() 69 | endforeach() 70 | 71 | 72 | 73 | 74 | file(GLOB_RECURSE BTHeadLibrary include/*.h) 75 | 76 | set(BTSrcLibrary 77 | src/action_node.cpp 78 | src/behavior_tree.cpp 79 | src/condition_node.cpp 80 | src/control_node.cpp 81 | src/draw.cpp 82 | src/exceptions.cpp 83 | src/leaf_node.cpp 84 | src/tick_engine.cpp 85 | src/parallel_node.cpp 86 | src/fallback_node.cpp 87 | src/sequence_node.cpp 88 | src/fallback_node_with_memory.cpp 89 | src/sequence_node_with_memory.cpp 90 | src/tree_node.cpp 91 | 92 | gtest/src/action_test_node.cpp 93 | gtest/src/condition_test_node.cpp 94 | ) 95 | 96 | 97 | ###################################################### 98 | # SETTING BINARIES AND LIBRARY OUTPUT LOCATIONS 99 | ###################################################### 100 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/sample) 101 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) 102 | 103 | ###################################################### 104 | # COMPILING GTEST 105 | ###################################################### 106 | if(GTEST_FOUND) 107 | #add_executable(btpp_gtest gtest/gtest_tree.cpp ${BTSrcLibrary} ${BTHeadLibrary}) 108 | #target_link_libraries(btpp_gtest ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES} ${X11_LIBRARIES}) 109 | endif(GTEST_FOUND) 110 | 111 | ###################################################### 112 | # COMPILING SAMPLE EXAMPLE 113 | ###################################################### 114 | add_executable(btpp_example src/example.cpp ${BTSrcLibrary} ${BTHeadLibrary}) 115 | target_link_libraries(btpp_example ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${X11_LIBRARIES}) 116 | 117 | ###################################################### 118 | # COMPILING LIBRARY 119 | ###################################################### 120 | add_library(BTppLib STATIC ${BTSrcLibrary} ${BTHeadLibrary}) 121 | target_link_libraries(BTppLib ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${X11_LIBRARIES}) 122 | 123 | ###################################################### 124 | # INSTALLATION OF LIBRARY AND EXECUTABLE 125 | ###################################################### 126 | 127 | 128 | 129 | # Add all targets to the build-tree export set 130 | export(TARGETS BTppLib 131 | FILE "${PROJECT_BINARY_DIR}/BTppTargets.cmake") 132 | 133 | # Export the package for use from the build-tree 134 | # (this registers the build-tree with a global CMake-registry) 135 | export(PACKAGE BTpp) 136 | 137 | 138 | install(FILES ${BTHeadLibrary} DESTINATION ${INSTALL_INCLUDE_DIR}/include/BTpp) 139 | 140 | 141 | install(TARGETS BTppLib 142 | # IMPORTANT: Add the library to the "export-set" 143 | EXPORT BTppTargets 144 | ARCHIVE DESTINATION "${INSTALL_CMAKE_DIR}/BTpp/lib" 145 | RUNTIME DESTINATION "${PROJECT_BINARY_DIR}/bin" COMPONENT ${PROJECT_BINARY_DIR}/bin 146 | LIBRARY DESTINATION "${PROJECT_BINARY_DIR}/lib" COMPONENT ${PROJECT_BINARY_DIR}/shlib 147 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/include/BTpp" 148 | COMPONENT ${INSTALL_CMAKE_DIR}"include/BTpp") 149 | 150 | 151 | # Create the BTppConfig.cmake and BTppConfigVersion files 152 | file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" 153 | "${INSTALL_INCLUDE_DIR}") 154 | # ... for the build tree 155 | 156 | 157 | set(CONF_INCLUDE_DIRS "${INSTALL_INCLUDE_DIR}include/BTpp") 158 | configure_file(conf/BTppConfig.cmake.in 159 | "${PROJECT_BINARY_DIR}/BTppConfig.cmake" @ONLY) 160 | 161 | # ... for the install tree 162 | #set(CONF_INCLUDE_DIRS "\${BTpp_CMAKE_DIR}/${REL_INCLUDE_DIR}") 163 | configure_file(conf/BTppConfig.cmake.in 164 | "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/BTppConfig.cmake" @ONLY) 165 | # ... for both 166 | configure_file(conf/BTppConfigVersion.cmake.in 167 | "${PROJECT_BINARY_DIR}/BTppConfigVersion.cmake" @ONLY) 168 | 169 | # Install the BTppConfig.cmake and BTppConfigVersion.cmake 170 | install(FILES 171 | "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/BTppConfig.cmake" 172 | "${PROJECT_BINARY_DIR}/BTppConfigVersion.cmake" 173 | DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT dev) 174 | 175 | # Install the export set for use with the install-tree 176 | install(EXPORT BTppTargets DESTINATION 177 | "${INSTALL_CMAKE_DIR}" COMPONENT dev) 178 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 - 2018 Michele Colledanchise 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | A lightweight behavior tree library in `C++`. 2 | 3 | 4 | 5 | NEWS! 6 | ----------- 7 | :boom: Thanks to [Davide Faconti](https://github.com/facontidavide) there is now a more sophisticated version of the library. The new version of this library is available [here](https://github.com/BehaviorTree/BehaviorTree.CPP). There is also GUI available [here](https://github.com/BehaviorTree/Groot). 8 | 9 | 10 | 11 | 12 | Our book **Behavior Trees in Robotics and AI**, published by CRC Press Taylor & Francis, is available for purchase (ebook and hardcover) on the CRC Press Store or Amazon. The Preprint version (**free**) is available here: https://arxiv.org/abs/1709.00084

13 | **Tutorials** available at https://btirai.github.io/ 14 | 15 | 16 | 17 |









18 | 19 | ----------- 20 | 21 | portfolio_view BT++ 22 | ==== 23 | ![License MIT](https://img.shields.io/dub/l/vibe-d.svg) ![Version](https://img.shields.io/badge/version-v1.4-green.svg) 24 |
25 | 26 | 27 | REFERENCE 28 | ------------ 29 | Please refer to the following paper when using the library: 30 | 31 | **How Behavior Trees Modularize Hybrid Control Systems and Generalize Sequential Behavior Compositions, the Subsumption Architecture, and Decision Trees.** Michele Colledanchise and Petter Ogren. IEEE Transaction on Robotics 2017. 32 | 33 | bibtex entry: 34 | 35 | `@ARTICLE{TRO17Colledanchise,`
36 | `author={M. Colledanchise and P. Ögren},`
37 | `journal={IEEE Transactions on Robotics},`
38 | `title={{How Behavior Trees Modularize Hybrid Control Systems and Generalize Sequential Behavior Compositions, the Subsumption Architecture, and Decision Trees}},`
39 | `year={2017},`
40 | `volume={33},`
41 | `number={2},`
42 | `pages={372-389},`
43 | `keywords={Computer architecture;Decision trees;High definition video;Robot control;Switches;Behavior trees (BTs);decision trees;finite state machines (FSMs);hybrid dynamical systems (HDSs);modularity;sequential behavior compositions;subsumption architecture}, `
44 | `doi={10.1109/TRO.2016.2633567},`
45 | `ISSN={1552-3098},`
46 | `month={April},}`
47 | 48 | DEPENDENCIES 49 | ------------ 50 | 51 | Regarding visualization purposes: 52 | * [OpenGL](https://www.opengl.org/) 53 | * [Glut](https://www.opengl.org/resources/libraries/glut/) 54 | 55 | Regarding tests: 56 | * [GTests](https://github.com/google/googletest) 57 | 58 | BT NODES SUPPORT 59 | ---------------- 60 | **Fallback:** Fallback nodes are used to find and execute the first child that does not fail. A Selector node will return immediately with a status code of success or running when one of its children returns success or running. The children are ticked in order of importance, from `left` to `right`. 61 | 62 | **Sequence:** Sequence nodes are used to find and execute the first child that has not yet succeeded. A sequence node will return immediately with a status code of `failure` or `running` when one of its children returns failure or running. The children are ticked in order, from `left` to `right`. 63 | 64 | **Parallel:** The parallel node ticks its children in parallel and returns success if `M ≤ N` children return success, it returns failure if `N − M + 1` children return failure, and it returns running otherwise. 65 | 66 | **Decorator:** The decorator node manipulates the return status of its child according to the policy defined by the user (e.g. it inverts the success/failure status of the child). In this library the decorators implemented are the two common ones: *Decorator Retry* which retries the execution of a node if this fails; and *Decorator Negation* That inverts the Success/Failure outcome. 67 | 68 | **Action:** An Action node performs an action, and returns Success if the action is completed, Failure if it can not be completed and Running if completion is under way. 69 | 70 | **Condition:** A Condition node determines if a desired condition `c` has been met. Conditions are technically a subset of the Actions, but are given a separate category and graphical symbol to improve readability of the BT and emphasize the fact that they never return running and do not change any internal states/variables of the BT. 71 | 72 | A user manual is available in the project folder ([BTppUserManual.pdf](https://github.com/miccol/Behavior-Tree/blob/master/BTppUserManual.pdf)). 73 | 74 | SETUP 75 | ----------- 76 | 77 | The first step to use BT++ is to retrieve its source code. You can either download it 78 | here (https://github.com/miccol/Behavior-Tree) or clone the repository: 79 | 80 | `$ cd /path/to/folder`
81 | `$ git clone https://github.com/miccol/Behavior−Tree.git` 82 | 83 | Once you have the repository, compile the library: 84 | 85 | `$ cd /path/to/folder/`
86 | `$ mkdir ./build`
87 | `$ cd build`
88 | `$ cmake ..`
89 | `$ make`
90 | 91 | 92 | **NOTE** 93 | In case you get the following error: 94 | 95 | `CMake Error: The following variables are used in this project, but they are set to NOTFOUND. 96 | Please set them or make sure they are set and tested correctly in the CMake files: 97 | GLUT_Xmu_LIBRARY (ADVANCED)` 98 | 99 | please see solution [here](https://ubuntuforums.org/archive/index.php/t-1703770.html). Thanks [miquelramirez](https://github.com/miquelramirez) for this. 100 | 101 | 102 | Check the installation by running a sample example. 103 | 104 | `$ cd /path/to/folder/`
105 | `$ cd build/sample`
106 | `$ ./btpp_example`
107 | 108 | INSTALL THE LIBRARY SYSTEM-WIDE (tested on Ubuntu 14.04 and 16.04) 109 | ------------------------------- 110 | 111 | If you would like to install the library system-wide, then run: 112 | 113 | `$ cd /path/to/folder/`
114 | `$ cd build`
115 | `$ sudo make install`
116 | 117 | On Ubuntu, this will install the library (libbtpp.so) in `/usr/local/lib`.
118 | In an external project, just call in your CMakeLists 'find_package(BTpp)' to find the library.
119 | The include directory is defined as `BTpp_INCLUDE_DIRS` and the libraries to link as `BTpp_LIBRARIES`.
120 | The repository [my-behavior-tree-project](https://github.com/miccol/my-behavior-tree-project) shows an example on how to use the library once system-wide installed. 121 | 122 | 123 | CREATE YOUR OWN ACTION NODE 124 | ------ 125 | 1) Implement your action node class extending the abstract class `BT::ActionNode`. 126 | 2) Implement the method `BT::ReturnStatus Tick()` with the code you want to execute while the action is running. Use the method `is_halted()` to check if the action has been prempted. When the execution of your action finished, return `BT::SUCCESS` or `BT::FAILURE` accordingly. 127 | 3) Implement the method `void Halt()` with the code you want to execute when the action gets preempted (halted). 128 | See the file `src/example.cpp` for an example. 129 | 130 | CREATE YOUR OWN CONDITION NODE 131 | ------ 132 | 1) Implement your condition node class extending the abstract class `BT::ConditionNode`. 133 | 2) Implement the method `BT::ReturnStatus Tick()` with the code you want to execute to check the condition. Return `BT::SUCCESS` or `BT::FAILURE` accordingly. 134 | See the file `src/example.cpp` for an example. 135 | 136 | 137 | NOTES 138 | ------- 139 | In case you are puzzled about why a sequence (or fallback) node with 2 or more actions as children never get past the first action, see [this](https://github.com/miccol/ROS-Behavior-Tree/issues/16) discussion. 140 | 141 | LICENSE 142 | ------- 143 | The MIT License (MIT) 144 | 145 | Copyright (c) 2014-2018 Michele Colledanchise 146 | 147 | Permission is hereby granted, free of charge, to any person obtaining a copy 148 | of this software and associated documentation files (the "Software"), to deal 149 | in the Software without restriction, including without limitation the rights 150 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 151 | copies of the Software, and to permit persons to whom the Software is 152 | furnished to do so, subject to the following conditions: 153 | 154 | The above copyright notice and this permission notice shall be included in all 155 | copies or substantial portions of the Software. 156 | 157 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 158 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 159 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 160 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 161 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 162 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 163 | SOFTWARE. 164 | -------------------------------------------------------------------------------- /conf/BTppConfig.cmake.in: -------------------------------------------------------------------------------- 1 | get_filename_component(BTpp_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 2 | set(BTpp_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 3 | 4 | # Our library dependencies (contains definitions for IMPORTED targets) 5 | if(NOT TARGET BTpp AND NOT BTpp_BINARY_DIR) 6 | include("${BTpp_CMAKE_DIR}/BTppTargets.cmake") 7 | endif() 8 | 9 | # These are IMPORTED targets created by YARPBTCoreargets.cmake 10 | set(BTpp_LIBRARIES BTppLib) 11 | -------------------------------------------------------------------------------- /conf/BTppConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION "@BTpp_VERSION@") 2 | 3 | # Check whether the requested PACKAGE_FIND_VERSION is compatible 4 | if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 5 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 6 | else() 7 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 8 | if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") 9 | set(PACKAGE_VERSION_EXACT TRUE) 10 | endif() 11 | endif() 12 | -------------------------------------------------------------------------------- /contributors.txt: -------------------------------------------------------------------------------- 1 | Michele Colledanchise 2 | Rocco Santomo 3 | 4 | -------------------------------------------------------------------------------- /gtest/gtest_tree.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | 21 | struct SimpleSequenceTest : testing::Test 22 | { 23 | BT:: SequenceNode* root; 24 | BT::ActionTestNode* action; 25 | BT::ConditionTestNode* condition; 26 | SimpleSequenceTest() 27 | { 28 | action = new BT::ActionTestNode("action"); 29 | condition = new BT::ConditionTestNode("condition"); 30 | 31 | root = new BT::SequenceNode("seq1"); 32 | 33 | root->AddChild(condition); 34 | root->AddChild(action); 35 | } 36 | }; 37 | 38 | 39 | struct ComplexSequenceTest : testing::Test 40 | { 41 | BT:: SequenceNode* root; 42 | BT::ActionTestNode* action_1; 43 | BT::ConditionTestNode* condition_1; 44 | BT::ConditionTestNode* condition_2; 45 | 46 | BT:: SequenceNode* seq_conditions; 47 | 48 | ComplexSequenceTest() 49 | { 50 | action_1 = new BT::ActionTestNode("action 1"); 51 | 52 | condition_1 = new BT::ConditionTestNode("condition 1"); 53 | condition_2 = new BT::ConditionTestNode("condition 2"); 54 | seq_conditions = new BT::SequenceNode("sequence_conditions"); 55 | 56 | seq_conditions->AddChild(condition_1); 57 | seq_conditions->AddChild(condition_2); 58 | 59 | root = new BT::SequenceNode("root"); 60 | root->AddChild(seq_conditions); 61 | root->AddChild(action_1); 62 | } 63 | }; 64 | 65 | 66 | struct ComplexSequence2ActionsTest : testing::Test 67 | { 68 | BT:: SequenceNode* root; 69 | BT::ActionTestNode* action_1; 70 | BT::ActionTestNode* action_2; 71 | 72 | BT::ConditionTestNode* condition_1; 73 | BT::ConditionTestNode* condition_2; 74 | 75 | BT:: SequenceNode* seq_1; 76 | BT:: SequenceNode* seq_2; 77 | 78 | ComplexSequence2ActionsTest() 79 | { 80 | action_1 = new BT::ActionTestNode("action 1"); 81 | action_2 = new BT::ActionTestNode("action 2"); 82 | seq_1 = new BT::SequenceNode("sequence_1"); 83 | seq_2 = new BT::SequenceNode("sequence_c2"); 84 | 85 | condition_1 = new BT::ConditionTestNode("condition 1"); 86 | condition_2 = new BT::ConditionTestNode("condition 2"); 87 | 88 | seq_1->AddChild(condition_1); 89 | seq_1->AddChild(action_1); 90 | 91 | seq_2->AddChild(condition_2); 92 | seq_2->AddChild(action_2); 93 | 94 | 95 | root = new BT::SequenceNode("root"); 96 | root->AddChild(seq_1); 97 | root->AddChild(seq_2); 98 | } 99 | }; 100 | 101 | 102 | struct SimpleFallbackTest : testing::Test 103 | { 104 | BT:: FallbackNode* root; 105 | BT::ActionTestNode* action; 106 | BT::ConditionTestNode* condition; 107 | SimpleFallbackTest() 108 | { 109 | action = new BT::ActionTestNode("action"); 110 | condition = new BT::ConditionTestNode("condition"); 111 | 112 | root = new BT::FallbackNode("seq1"); 113 | 114 | root->AddChild(condition); 115 | root->AddChild(action); 116 | } 117 | }; 118 | 119 | 120 | struct ComplexFallbackTest : testing::Test 121 | { 122 | BT:: FallbackNode* root; 123 | BT::ActionTestNode* action_1; 124 | BT::ConditionTestNode* condition_1; 125 | BT::ConditionTestNode* condition_2; 126 | 127 | BT:: FallbackNode* sel_conditions; 128 | 129 | ComplexFallbackTest() 130 | { 131 | action_1 = new BT::ActionTestNode("action 1"); 132 | condition_1 = new BT::ConditionTestNode("condition 1"); 133 | condition_2 = new BT::ConditionTestNode("condition 2"); 134 | sel_conditions = new BT::FallbackNode("fallback_conditions"); 135 | 136 | sel_conditions->AddChild(condition_1); 137 | sel_conditions->AddChild(condition_2); 138 | 139 | root = new BT::FallbackNode("root"); 140 | root->AddChild(sel_conditions); 141 | root->AddChild(action_1); 142 | } 143 | }; 144 | 145 | 146 | 147 | 148 | struct BehaviorTreeTest : testing::Test 149 | { 150 | BT:: SequenceNode* root; 151 | BT::ActionTestNode* action_1; 152 | BT::ConditionTestNode* condition_1; 153 | BT::ConditionTestNode* condition_2; 154 | 155 | BT:: FallbackNode* sel_conditions; 156 | 157 | BehaviorTreeTest() 158 | { 159 | action_1 = new BT::ActionTestNode("action 1"); 160 | condition_1 = new BT::ConditionTestNode("condition 1"); 161 | condition_2 = new BT::ConditionTestNode("condition 2"); 162 | sel_conditions = new BT::FallbackNode("fallback_conditions"); 163 | 164 | sel_conditions->AddChild(condition_1); 165 | sel_conditions->AddChild(condition_2); 166 | 167 | root = new BT::SequenceNode("root"); 168 | root->AddChild(sel_conditions); 169 | root->AddChild(action_1); 170 | } 171 | }; 172 | 173 | 174 | 175 | 176 | struct SimpleSequenceWithMemoryTest : testing::Test 177 | { 178 | BT:: SequenceNodeWithMemory* root; 179 | BT::ActionTestNode* action; 180 | BT::ConditionTestNode* condition; 181 | SimpleSequenceWithMemoryTest() 182 | { 183 | action = new BT::ActionTestNode("action"); 184 | condition = new BT::ConditionTestNode("condition"); 185 | 186 | root = new BT::SequenceNodeWithMemory("seq1"); 187 | 188 | root->AddChild(condition); 189 | root->AddChild(action); 190 | } 191 | }; 192 | 193 | struct ComplexSequenceWithMemoryTest : testing::Test 194 | { 195 | BT:: SequenceNodeWithMemory* root; 196 | 197 | BT::ActionTestNode* action_1; 198 | BT::ActionTestNode* action_2; 199 | 200 | BT::ConditionTestNode* condition_1; 201 | BT::ConditionTestNode* condition_2; 202 | 203 | BT:: SequenceNodeWithMemory* seq_conditions; 204 | BT:: SequenceNodeWithMemory* seq_actions; 205 | 206 | ComplexSequenceWithMemoryTest() 207 | { 208 | action_1 = new BT::ActionTestNode("action 1"); 209 | action_2 = new BT::ActionTestNode("action 2"); 210 | 211 | 212 | condition_1 = new BT::ConditionTestNode("condition 1"); 213 | condition_2 = new BT::ConditionTestNode("condition 2"); 214 | 215 | seq_conditions = new BT::SequenceNodeWithMemory("sequence_conditions"); 216 | seq_actions = new BT::SequenceNodeWithMemory("sequence_actions"); 217 | 218 | seq_actions->AddChild(action_1); 219 | seq_actions->AddChild(action_2); 220 | 221 | seq_conditions->AddChild(condition_1); 222 | seq_conditions->AddChild(condition_2); 223 | 224 | root = new BT::SequenceNodeWithMemory("root"); 225 | root->AddChild(seq_conditions); 226 | root->AddChild(seq_actions); 227 | } 228 | }; 229 | 230 | struct SimpleFallbackWithMemoryTest : testing::Test 231 | { 232 | BT::FallbackNodeWithMemory* root; 233 | BT::ActionTestNode* action; 234 | BT::ConditionTestNode* condition; 235 | SimpleFallbackWithMemoryTest() 236 | { 237 | action = new BT::ActionTestNode("action"); 238 | condition = new BT::ConditionTestNode("condition"); 239 | 240 | root = new BT::FallbackNodeWithMemory("seq1"); 241 | 242 | root->AddChild(condition); 243 | root->AddChild(action); 244 | } 245 | }; 246 | 247 | struct ComplexFallbackWithMemoryTest : testing::Test 248 | { 249 | BT:: FallbackNodeWithMemory* root; 250 | 251 | BT::ActionTestNode* action_1; 252 | BT::ActionTestNode* action_2; 253 | 254 | BT::ConditionTestNode* condition_1; 255 | BT::ConditionTestNode* condition_2; 256 | 257 | BT:: FallbackNodeWithMemory* fal_conditions; 258 | BT:: FallbackNodeWithMemory* fal_actions; 259 | 260 | ComplexFallbackWithMemoryTest() 261 | { 262 | action_1 = new BT::ActionTestNode("action 1"); 263 | action_2 = new BT::ActionTestNode("action 2"); 264 | condition_1 = new BT::ConditionTestNode("condition 1"); 265 | condition_2 = new BT::ConditionTestNode("condition 2"); 266 | 267 | fal_conditions = new BT::FallbackNodeWithMemory("fallback_conditions"); 268 | fal_actions = new BT::FallbackNodeWithMemory("fallback_actions"); 269 | 270 | fal_actions->AddChild(action_1); 271 | fal_actions->AddChild(action_2); 272 | 273 | fal_conditions->AddChild(condition_1); 274 | fal_conditions->AddChild(condition_2); 275 | 276 | root = new BT::FallbackNodeWithMemory("root"); 277 | root->AddChild(fal_conditions); 278 | root->AddChild(fal_actions); 279 | } 280 | }; 281 | 282 | 283 | struct SimpleParallelTest : testing::Test 284 | { 285 | BT::ParallelNode* root; 286 | BT::ActionTestNode* action_1; 287 | BT::ConditionTestNode* condition_1; 288 | 289 | BT::ActionTestNode* action_2; 290 | BT::ConditionTestNode* condition_2; 291 | 292 | SimpleParallelTest() 293 | { 294 | action_1 = new BT::ActionTestNode("action 1"); 295 | condition_1 = new BT::ConditionTestNode("condition 1"); 296 | 297 | 298 | action_2 = new BT::ActionTestNode("action 2"); 299 | condition_2 = new BT::ConditionTestNode("condition 2"); 300 | 301 | root = new BT::ParallelNode("par", 4); 302 | 303 | root->AddChild(condition_1); 304 | root->AddChild(action_1); 305 | root->AddChild(condition_2); 306 | root->AddChild(action_2); 307 | } 308 | }; 309 | 310 | 311 | struct ComplexParallelTest : testing::Test 312 | { 313 | BT::ParallelNode* root; 314 | BT::ParallelNode* parallel_1; 315 | BT::ParallelNode* parallel_2; 316 | 317 | BT::ActionTestNode* action_1; 318 | BT::ConditionTestNode* condition_1; 319 | 320 | BT::ActionTestNode* action_2; 321 | BT::ConditionTestNode* condition_2; 322 | 323 | BT::ActionTestNode* action_3; 324 | BT::ConditionTestNode* condition_3; 325 | 326 | ComplexParallelTest() 327 | { 328 | action_1 = new BT::ActionTestNode("action 1"); 329 | condition_1 = new BT::ConditionTestNode("condition 1"); 330 | 331 | 332 | action_2 = new BT::ActionTestNode("action 2"); 333 | condition_2 = new BT::ConditionTestNode("condition 2"); 334 | 335 | 336 | action_3 = new BT::ActionTestNode("action 3"); 337 | condition_3 = new BT::ConditionTestNode("condition 3"); 338 | 339 | root = new BT::ParallelNode("root", 2); 340 | parallel_1 = new BT::ParallelNode("par1", 3); 341 | parallel_2 = new BT::ParallelNode("par2", 1); 342 | 343 | parallel_1->AddChild(condition_1); 344 | parallel_1->AddChild(action_1); 345 | parallel_1->AddChild(condition_2); 346 | parallel_1->AddChild(action_2); 347 | 348 | parallel_2->AddChild(condition_3); 349 | parallel_2->AddChild(action_3); 350 | 351 | root->AddChild(parallel_1); 352 | root->AddChild(parallel_2); 353 | } 354 | }; 355 | 356 | 357 | 358 | /****************TESTS START HERE***************************/ 359 | 360 | 361 | 362 | TEST_F(SimpleSequenceTest, ConditionTrue) 363 | { 364 | std::cout << "Ticking the root node !" << std::endl << std::endl; 365 | // Ticking the root node 366 | BT::ReturnStatus state = root->Tick(); 367 | 368 | ASSERT_EQ(BT::RUNNING, action->get_status()); 369 | ASSERT_EQ(BT::RUNNING, state); 370 | root->Halt(); 371 | } 372 | 373 | 374 | TEST_F(SimpleSequenceTest, ConditionTurnToFalse) 375 | { 376 | BT::ReturnStatus state = root->Tick(); 377 | condition->set_boolean_value(false); 378 | 379 | state = root->Tick(); 380 | ASSERT_EQ(BT::FAILURE, state); 381 | ASSERT_EQ(BT::HALTED, action->get_status()); 382 | root->Halt(); 383 | } 384 | 385 | 386 | TEST_F(ComplexSequenceTest, ComplexSequenceConditionsTrue) 387 | { 388 | BT::ReturnStatus state = root->Tick(); 389 | 390 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 391 | ASSERT_EQ(BT::RUNNING, state); 392 | root->Halt(); 393 | } 394 | 395 | 396 | TEST_F(ComplexSequence2ActionsTest, ConditionsTrue) 397 | { 398 | BT::ReturnStatus state = root->Tick(); 399 | 400 | state = root->Tick(); 401 | 402 | std::this_thread::sleep_for(std::chrono::seconds(5)); 403 | state = root->Tick(); 404 | state = root->Tick(); 405 | 406 | 407 | ASSERT_EQ(BT::RUNNING, state); 408 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 409 | ASSERT_EQ(BT::RUNNING, seq_1->get_status()); 410 | ASSERT_EQ(BT::HALTED, seq_2->get_status()); 411 | ASSERT_EQ(BT::HALTED, action_2->get_status()); 412 | 413 | root->Halt(); 414 | } 415 | 416 | TEST_F(ComplexSequenceTest, ComplexSequenceConditions1ToFalse) 417 | { 418 | BT::ReturnStatus state = root->Tick(); 419 | 420 | condition_1->set_boolean_value(false); 421 | 422 | state = root->Tick(); 423 | 424 | ASSERT_EQ(BT::FAILURE, state); 425 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 426 | root->Halt(); 427 | } 428 | 429 | TEST_F(ComplexSequenceTest, ComplexSequenceConditions2ToFalse) 430 | { 431 | BT::ReturnStatus state = root->Tick(); 432 | 433 | condition_2->set_boolean_value(false); 434 | 435 | state = root->Tick(); 436 | 437 | ASSERT_EQ(BT::FAILURE, state); 438 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 439 | root->Halt(); 440 | } 441 | 442 | 443 | 444 | TEST_F(SimpleFallbackTest, ConditionTrue) 445 | { 446 | std::cout << "Ticking the root node !" << std::endl << std::endl; 447 | // Ticking the root node 448 | condition->set_boolean_value(true); 449 | BT::ReturnStatus state = root->Tick(); 450 | 451 | ASSERT_EQ(BT::IDLE, action->get_status()); 452 | ASSERT_EQ(BT::SUCCESS, state); 453 | root->Halt(); 454 | } 455 | 456 | 457 | TEST_F(SimpleFallbackTest, ConditionToFalse) 458 | { 459 | condition->set_boolean_value(false); 460 | 461 | BT::ReturnStatus state = root->Tick(); 462 | condition->set_boolean_value(true); 463 | 464 | 465 | state = root->Tick(); 466 | 467 | ASSERT_EQ(BT::SUCCESS, state); 468 | ASSERT_EQ(BT::HALTED, action->get_status()); 469 | root->Halt(); 470 | } 471 | 472 | 473 | TEST_F(ComplexFallbackTest, Condition1ToTrue) 474 | { 475 | condition_1->set_boolean_value(false); 476 | condition_2->set_boolean_value(false); 477 | 478 | BT::ReturnStatus state = root->Tick(); 479 | 480 | condition_1->set_boolean_value(true); 481 | 482 | state = root->Tick(); 483 | 484 | ASSERT_EQ(BT::SUCCESS, state); 485 | 486 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 487 | root->Halt(); 488 | } 489 | 490 | TEST_F(ComplexFallbackTest, Condition2ToTrue) 491 | { 492 | condition_1->set_boolean_value(false); 493 | condition_2->set_boolean_value(false); 494 | 495 | BT::ReturnStatus state = root->Tick(); 496 | 497 | condition_2->set_boolean_value(true); 498 | 499 | state = root->Tick(); 500 | 501 | ASSERT_EQ(BT::SUCCESS, state); 502 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 503 | root->Halt(); 504 | } 505 | 506 | 507 | 508 | TEST_F(BehaviorTreeTest, Condition1ToFalseCondition2True) 509 | { 510 | condition_1->set_boolean_value(false); 511 | condition_2->set_boolean_value(true); 512 | 513 | BT::ReturnStatus state = root->Tick(); 514 | 515 | ASSERT_EQ(BT::RUNNING, state); 516 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 517 | 518 | root->Halt(); 519 | } 520 | 521 | TEST_F(BehaviorTreeTest, Condition2ToFalseCondition1True) 522 | { 523 | condition_2->set_boolean_value(false); 524 | condition_1->set_boolean_value(true); 525 | 526 | BT::ReturnStatus state = root->Tick(); 527 | 528 | ASSERT_EQ(BT::RUNNING, state); 529 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 530 | 531 | root->Halt(); 532 | } 533 | 534 | 535 | TEST_F(SimpleSequenceWithMemoryTest, ConditionTrue) 536 | { 537 | std::cout << "Ticking the root node !" << std::endl << std::endl; 538 | // Ticking the root node 539 | BT::ReturnStatus state = root->Tick(); 540 | std::this_thread::sleep_for(std::chrono::seconds(1)); 541 | 542 | ASSERT_EQ(BT::RUNNING, action->get_status()); 543 | ASSERT_EQ(BT::RUNNING, state); 544 | root->Halt(); 545 | } 546 | 547 | 548 | TEST_F(SimpleSequenceWithMemoryTest, ConditionTurnToFalse) 549 | { 550 | BT::ReturnStatus state = root->Tick(); 551 | 552 | condition->set_boolean_value(false); 553 | 554 | state = root->Tick(); 555 | 556 | ASSERT_EQ(BT::RUNNING, state); 557 | ASSERT_EQ(BT::RUNNING, action->get_status()); 558 | 559 | root->Halt(); 560 | } 561 | 562 | 563 | TEST_F(ComplexSequenceWithMemoryTest, ConditionsTrue) 564 | { 565 | BT::ReturnStatus state = root->Tick(); 566 | 567 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 568 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 569 | ASSERT_EQ(BT::RUNNING, state); 570 | 571 | root->Halt(); 572 | } 573 | 574 | 575 | TEST_F(ComplexSequenceWithMemoryTest, Conditions1ToFalse) 576 | { 577 | BT::ReturnStatus state = root->Tick(); 578 | 579 | condition_1->set_boolean_value(false); 580 | 581 | state = root->Tick(); 582 | 583 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 584 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 585 | ASSERT_EQ(BT::RUNNING, state); 586 | root->Halt(); 587 | } 588 | 589 | TEST_F(ComplexSequenceWithMemoryTest, Conditions2ToFalse) 590 | { 591 | BT::ReturnStatus state = root->Tick(); 592 | 593 | condition_2->set_boolean_value(false); 594 | 595 | state = root->Tick(); 596 | 597 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 598 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 599 | ASSERT_EQ(BT::RUNNING, state); 600 | 601 | root->Halt(); 602 | } 603 | 604 | TEST_F(ComplexSequenceWithMemoryTest, Action1Done) 605 | { 606 | root->Tick(); 607 | 608 | condition_2->set_boolean_value(false); 609 | 610 | root->Tick(); 611 | std::this_thread::sleep_for(std::chrono::seconds(10)); 612 | root->Tick(); 613 | 614 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 615 | 616 | root->Halt(); 617 | } 618 | 619 | TEST_F(SimpleFallbackWithMemoryTest, ConditionFalse) 620 | { 621 | std::cout << "Ticking the root node !" << std::endl << std::endl; 622 | // Ticking the root node 623 | condition->set_boolean_value(false); 624 | BT::ReturnStatus state = root->Tick(); 625 | std::this_thread::sleep_for(std::chrono::seconds(1)); 626 | 627 | ASSERT_EQ(BT::RUNNING, action->get_status()); 628 | ASSERT_EQ(BT::RUNNING, state); 629 | 630 | root->Halt(); 631 | } 632 | 633 | 634 | TEST_F(SimpleFallbackWithMemoryTest, ConditionTurnToTrue) 635 | { 636 | condition->set_boolean_value(false); 637 | 638 | BT::ReturnStatus state = root->Tick(); 639 | condition->set_boolean_value(true); 640 | 641 | state = root->Tick(); 642 | 643 | ASSERT_EQ(BT::RUNNING, state); 644 | ASSERT_EQ(BT::RUNNING, action->get_status()); 645 | 646 | root->Halt(); 647 | } 648 | 649 | 650 | 651 | TEST_F(ComplexFallbackWithMemoryTest, ConditionsTrue) 652 | { 653 | BT::ReturnStatus state = root->Tick(); 654 | 655 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 656 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 657 | ASSERT_EQ(BT::SUCCESS, state); 658 | 659 | root->Halt(); 660 | } 661 | 662 | TEST_F(ComplexFallbackWithMemoryTest, Condition1False) 663 | { 664 | condition_1->set_boolean_value(false); 665 | BT::ReturnStatus state = root->Tick(); 666 | 667 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 668 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 669 | ASSERT_EQ(BT::SUCCESS, state); 670 | 671 | root->Halt(); 672 | } 673 | 674 | 675 | TEST_F(ComplexFallbackWithMemoryTest, ConditionsFalse) 676 | { 677 | condition_1->set_boolean_value(false); 678 | condition_2->set_boolean_value(false); 679 | BT::ReturnStatus state = root->Tick(); 680 | 681 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 682 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 683 | ASSERT_EQ(BT::RUNNING, state); 684 | 685 | root->Halt(); 686 | } 687 | 688 | 689 | 690 | 691 | TEST_F(ComplexFallbackWithMemoryTest, Conditions1ToTrue) 692 | { 693 | condition_1->set_boolean_value(false); 694 | condition_2->set_boolean_value(false); 695 | BT::ReturnStatus state = root->Tick(); 696 | condition_1->set_boolean_value(true); 697 | 698 | state = root->Tick(); 699 | 700 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 701 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 702 | ASSERT_EQ(BT::RUNNING, state); 703 | 704 | root->Halt(); 705 | } 706 | 707 | TEST_F(ComplexFallbackWithMemoryTest, Conditions2ToTrue) 708 | { 709 | condition_1->set_boolean_value(false); 710 | 711 | condition_2->set_boolean_value(false); 712 | 713 | BT::ReturnStatus state = root->Tick(); 714 | 715 | condition_2->set_boolean_value(true); 716 | 717 | state = root->Tick(); 718 | 719 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 720 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 721 | ASSERT_EQ(BT::RUNNING, state); 722 | 723 | root->Halt(); 724 | } 725 | 726 | TEST_F(ComplexFallbackWithMemoryTest, Action1Failed) 727 | { 728 | action_1->set_boolean_value(false); 729 | condition_1->set_boolean_value(false); 730 | condition_2->set_boolean_value(false); 731 | 732 | BT::ReturnStatus state = root->Tick(); 733 | 734 | state = root->Tick(); 735 | std::this_thread::sleep_for(std::chrono::seconds(5)); 736 | state = root->Tick(); 737 | 738 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 739 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 740 | ASSERT_EQ(BT::RUNNING, state); 741 | 742 | root->Halt(); 743 | } 744 | 745 | 746 | TEST_F(SimpleParallelTest, ConditionsTrue) 747 | { 748 | BT::ReturnStatus state = root->Tick(); 749 | 750 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 751 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 752 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 753 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 754 | ASSERT_EQ(BT::RUNNING, state); 755 | 756 | root->Halt(); 757 | } 758 | 759 | 760 | TEST_F(SimpleParallelTest, Threshold_3) 761 | { 762 | root->set_threshold_M(3); 763 | action_2->set_time(200); 764 | root->Tick(); 765 | std::this_thread::sleep_for(std::chrono::seconds(5)); 766 | BT::ReturnStatus state = root->Tick(); 767 | 768 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 769 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 770 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 771 | ASSERT_EQ(BT::HALTED, action_2->get_status()); 772 | ASSERT_EQ(BT::SUCCESS, state); 773 | 774 | root->Halt(); 775 | } 776 | 777 | 778 | TEST_F(SimpleParallelTest, Threshold_1) 779 | { 780 | root->set_threshold_M(1); 781 | BT::ReturnStatus state = root->Tick(); 782 | 783 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 784 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 785 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 786 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 787 | ASSERT_EQ(BT::SUCCESS, state); 788 | 789 | root->Halt(); 790 | } 791 | TEST_F(ComplexParallelTest, ConditionsTrue) 792 | { 793 | BT::ReturnStatus state = root->Tick(); 794 | 795 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 796 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 797 | ASSERT_EQ(BT::IDLE, condition_3->get_status()); 798 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 799 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 800 | ASSERT_EQ(BT::IDLE, action_3->get_status()); 801 | ASSERT_EQ(BT::RUNNING, parallel_1->get_status()); 802 | ASSERT_EQ(BT::IDLE, parallel_2->get_status()); 803 | ASSERT_EQ(BT::RUNNING, state); 804 | 805 | root->Halt(); 806 | } 807 | 808 | 809 | 810 | TEST_F(ComplexParallelTest, Condition3False) 811 | { 812 | condition_3->set_boolean_value(false); 813 | BT::ReturnStatus state = root->Tick(); 814 | 815 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 816 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 817 | ASSERT_EQ(BT::IDLE, condition_3->get_status()); 818 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 819 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 820 | ASSERT_EQ(BT::RUNNING, action_3->get_status()); 821 | ASSERT_EQ(BT::RUNNING, parallel_1->get_status()); 822 | ASSERT_EQ(BT::RUNNING, parallel_2->get_status()); 823 | ASSERT_EQ(BT::RUNNING, state); 824 | 825 | root->Halt(); 826 | } 827 | 828 | 829 | TEST_F(ComplexParallelTest, Condition3FalseAction1Done) 830 | { 831 | action_2->set_time(10); 832 | action_3->set_time(10); 833 | 834 | condition_3->set_boolean_value(false); 835 | BT::ReturnStatus state = root->Tick(); 836 | std::this_thread::sleep_for(std::chrono::seconds(5)); 837 | 838 | 839 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 840 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 841 | ASSERT_EQ(BT::IDLE, condition_3->get_status()); 842 | ASSERT_EQ(BT::SUCCESS, action_1->get_status()); // success not read yet by the node parallel_1 843 | ASSERT_EQ(BT::RUNNING, parallel_1->get_status()); // parallel_1 hasn't realize (yet) that action_1 has succeeded 844 | 845 | state = root->Tick(); 846 | 847 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 848 | ASSERT_EQ(BT::IDLE, parallel_1->get_status()); 849 | ASSERT_EQ(BT::HALTED, action_2->get_status()); 850 | ASSERT_EQ(BT::RUNNING, action_3->get_status()); 851 | ASSERT_EQ(BT::RUNNING, parallel_2->get_status()); 852 | ASSERT_EQ(BT::RUNNING, state); 853 | 854 | 855 | state = root->Tick(); 856 | std::this_thread::sleep_for(std::chrono::seconds(15)); 857 | state = root->Tick(); 858 | 859 | ASSERT_EQ(BT::IDLE, parallel_2->get_status()); 860 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 861 | ASSERT_EQ(BT::IDLE, parallel_1->get_status()); 862 | ASSERT_EQ(BT::IDLE, action_3->get_status()); 863 | ASSERT_EQ(BT::IDLE, parallel_2->get_status()); 864 | ASSERT_EQ(BT::SUCCESS, state); 865 | 866 | root->Halt(); 867 | } 868 | 869 | int main(int argc, char **argv) 870 | { 871 | testing::InitGoogleTest(&argc, argv); 872 | return RUN_ALL_TESTS(); 873 | } 874 | 875 | 876 | -------------------------------------------------------------------------------- /gtest/include/action_test_node.h: -------------------------------------------------------------------------------- 1 | #ifndef ACTIONTEST_H 2 | #define ACTIONTEST_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class ActionTestNode : public ActionNode 9 | { 10 | 11 | public: 12 | // Constructor 13 | ActionTestNode(std::string Name); 14 | ~ActionTestNode(); 15 | 16 | // The method that is going to be executed by the thread 17 | BT::ReturnStatus Tick(); 18 | void set_time(int time); 19 | 20 | // The method used to interrupt the execution of the node 21 | void Halt(); 22 | void set_boolean_value(bool boolean_value); 23 | private: 24 | int time_; 25 | bool boolean_value_; 26 | 27 | ///ReturnStatus status_; 28 | 29 | }; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /gtest/include/condition_test_node.h: -------------------------------------------------------------------------------- 1 | #ifndef CONDITIONTEST_H 2 | #define CONDITIONTEST_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class ConditionTestNode : public ConditionNode 9 | { 10 | public: 11 | // Constructor 12 | ConditionTestNode(std::string Name); 13 | ~ConditionTestNode(); 14 | void set_boolean_value(bool boolean_value); 15 | 16 | // The method that is going to be executed by the thread 17 | BT::ReturnStatus Tick(); 18 | private: 19 | bool boolean_value_; 20 | }; 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /gtest/src/action_test_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | 18 | BT::ActionTestNode::ActionTestNode(std::string name) : ActionNode::ActionNode(name) 19 | { 20 | boolean_value_ = true; 21 | time_ = 3; 22 | } 23 | 24 | BT::ActionTestNode::~ActionTestNode() {} 25 | 26 | BT::ReturnStatus BT::ActionTestNode::Tick() 27 | { 28 | 29 | int i = 0; 30 | while (get_status() != BT::HALTED && i++ < time_) 31 | { 32 | DEBUG_STDOUT(" Action " << get_name() << "running! Thread id:" << std::this_thread::get_id()); 33 | std::this_thread::sleep_for(std::chrono::seconds(1)); 34 | } 35 | if (get_status() != BT::HALTED) 36 | { 37 | if (boolean_value_) 38 | { 39 | DEBUG_STDOUT(" Action " << get_name() << " Done!"); 40 | return BT::SUCCESS; 41 | } 42 | else 43 | { 44 | DEBUG_STDOUT(" Action " << get_name() << " FAILED!"); 45 | return BT::FAILURE; 46 | } 47 | } 48 | else 49 | { 50 | return BT::HALTED; 51 | } 52 | } 53 | 54 | void BT::ActionTestNode::Halt() 55 | { 56 | set_status(BT::HALTED); 57 | DEBUG_STDOUT("HALTED state set!"); 58 | } 59 | 60 | 61 | void BT::ActionTestNode::set_time(int time) 62 | { 63 | time_ = time; 64 | } 65 | 66 | 67 | 68 | void BT::ActionTestNode::set_boolean_value(bool boolean_value) 69 | { 70 | boolean_value_ = boolean_value; 71 | } 72 | 73 | 74 | -------------------------------------------------------------------------------- /gtest/src/condition_test_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | BT::ConditionTestNode::ConditionTestNode(std::string name) : ConditionNode::ConditionNode(name) 18 | { 19 | boolean_value_ = true; 20 | } 21 | 22 | BT::ConditionTestNode::~ConditionTestNode() {} 23 | 24 | BT::ReturnStatus BT::ConditionTestNode::Tick() 25 | { 26 | // Condition checking and state update 27 | 28 | if (boolean_value_) 29 | { 30 | set_status(BT::SUCCESS); 31 | return BT::SUCCESS; 32 | } 33 | else 34 | { 35 | set_status(BT::FAILURE); 36 | return BT::FAILURE; 37 | } 38 | } 39 | 40 | 41 | 42 | 43 | void BT::ConditionTestNode::set_boolean_value(bool boolean_value) 44 | { 45 | boolean_value_ = boolean_value; 46 | } 47 | 48 | -------------------------------------------------------------------------------- /include/action_node.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIORTREECORE_ACTIONNODE_H 2 | #define BEHAVIORTREECORE_ACTIONNODE_H 3 | 4 | #include "leaf_node.h" 5 | 6 | namespace BT 7 | { 8 | 9 | class ActionNode : public LeafNode 10 | { 11 | public: 12 | // Constructor 13 | ActionNode(std::string name); 14 | ~ActionNode(); 15 | 16 | // The method that is going to be executed by the thread 17 | void WaitForTick(); 18 | virtual BT::ReturnStatus Tick() = 0; 19 | 20 | // The method used to interrupt the execution of the node 21 | virtual void Halt() = 0; 22 | 23 | // Methods used to access the node state without the 24 | // conditional waiting (only mutual access) 25 | bool WriteState(ReturnStatus new_state); 26 | int DrawType(); 27 | }; 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /include/behavior_tree.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIORTREE_H 2 | #define BEHAVIORTREE_H 3 | 4 | 5 | 6 | 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include /* pow */ 28 | 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | void Execute(BT::ControlNode* root,int TickPeriod_milliseconds); 36 | 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /include/condition_node.h: -------------------------------------------------------------------------------- 1 | #ifndef CONDITIONNODE_H 2 | #define CONDITIONNODE_H 3 | 4 | #include "leaf_node.h" 5 | 6 | namespace BT 7 | { 8 | class ConditionNode : public LeafNode 9 | { 10 | public: 11 | // Constructor 12 | ConditionNode(std::string name); 13 | ~ConditionNode(); 14 | 15 | // The method that is going to be executed by the thread 16 | virtual BT::ReturnStatus Tick() = 0; 17 | 18 | // The method used to interrupt the execution of the node 19 | void Halt(); 20 | 21 | // Methods used to access the node state without the 22 | // conditional waiting (only mutual access) 23 | bool WriteState(ReturnStatus new_state); 24 | int DrawType(); 25 | }; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /include/control_node.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLNODE_H 2 | #define CONTROLNODE_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace BT 9 | { 10 | class ControlNode : public TreeNode 11 | { 12 | protected: 13 | // Children vector 14 | std::vector children_nodes_; 15 | 16 | // Children states 17 | std::vector children_states_; 18 | 19 | // Vector size 20 | unsigned int N_of_children_; 21 | //child i status. Used to rout the ticks 22 | ReturnStatus child_i_status_; 23 | 24 | public: 25 | // Constructor 26 | ControlNode(std::string name); 27 | ~ControlNode(); 28 | 29 | // The method used to fill the child vector 30 | void AddChild(TreeNode* child); 31 | 32 | // The method used to know the number of children 33 | unsigned int GetChildrenNumber(); 34 | std::vector GetChildren(); 35 | // The method used to interrupt the execution of the node 36 | void Halt(); 37 | void ResetColorState(); 38 | void HaltChildren(int i); 39 | int Depth(); 40 | 41 | // Methods used to access the node state without the 42 | // conditional waiting (only mutual access) 43 | bool WriteState(ReturnStatus new_state); 44 | }; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /include/decorator_negation_node.h: -------------------------------------------------------------------------------- 1 | #ifndef DECORATORNEGATIONNODE_H 2 | #define DECORATORNEGATIONNODE_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class DecoratorNegationNode : public ControlNode 9 | { 10 | public: 11 | // Constructor 12 | DecoratorNegationNode(std::string name); 13 | ~DecoratorNegationNode(); 14 | int DrawType(); 15 | // The method that is going to be executed by the thread 16 | void Exec(); 17 | void AddChild(TreeNode* child); 18 | }; 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /include/decorator_retry_node.h: -------------------------------------------------------------------------------- 1 | #ifndef DECORATORRETRYNODE_H 2 | #define DECORATORRETRYNODE_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class DecoratorRetryNode : public ControlNode 9 | { 10 | public: 11 | // Constructor 12 | DecoratorRetryNode(std::string name, unsigned int NTries); 13 | ~DecoratorRetryNode(); 14 | int DrawType(); 15 | // The method that is going to be executed by the thread 16 | void Exec(); 17 | private: 18 | unsigned int NTries_; 19 | unsigned int TryIndx_; 20 | }; 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /include/draw.h: -------------------------------------------------------------------------------- 1 | #ifndef DRAWTREE_H 2 | #define DRAWTREE_H 3 | #include 4 | 5 | #ifdef __APPLE__ 6 | #include 7 | #else 8 | #include 9 | #endif 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | //enum status {RUNNING,SUCCESS,FAILURE,IDLE, HALTED }; 17 | 18 | void drawEllipse(float xradius, float yradius); 19 | 20 | void drawTree(BT::ControlNode* tree_); 21 | 22 | void resize(int width, int height); 23 | 24 | void draw_status(float x, float y, int node_status); 25 | 26 | void drawString (void * font, char *string, float x, float y, float z); 27 | 28 | void renderBitmapString(float x, float y, void *font,const char *string); 29 | 30 | void draw_node(float x, float y, int node_type, const char *leafName, int status); 31 | 32 | void draw_edge(GLfloat parent_x, GLfloat parent_y, GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size); 33 | 34 | void keyboard(unsigned char key, int x, int y); 35 | 36 | void drawCircle(float radius); 37 | 38 | int compute_node_lines(const char *string); 39 | 40 | int compute_max_width(const char *string); 41 | 42 | //void display(); 43 | 44 | #endif // DRAWTREE_H 45 | -------------------------------------------------------------------------------- /include/exceptions.h: -------------------------------------------------------------------------------- 1 | #ifndef EXCEPTIONS_H 2 | #define EXCEPTIONS_H 3 | 4 | #include 5 | #include 6 | 7 | namespace BT 8 | { 9 | /// Exception class 10 | class BehaviorTreeException : public std::exception 11 | { 12 | private: 13 | const char* Message; 14 | public: 15 | BehaviorTreeException(const std::string Message); 16 | }; 17 | } 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /include/fallback_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FALLBACKNODE_H 2 | #define FALLBACKNODE_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class FallbackNode : public ControlNode 9 | { 10 | public: 11 | // Constructor 12 | FallbackNode(std::string name); 13 | ~FallbackNode(); 14 | int DrawType(); 15 | // The method that is going to be executed by the thread 16 | BT::ReturnStatus Tick(); 17 | }; 18 | } 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /include/fallback_node_with_memory.h: -------------------------------------------------------------------------------- 1 | #ifndef FALLBACK_NODE_WITH_MEMORY_H 2 | #define FALLBACK_NODE_WITH_MEMORY_H 3 | #include 4 | 5 | 6 | namespace BT 7 | { 8 | class FallbackNodeWithMemory : public ControlNode 9 | { 10 | public: 11 | // Constructor 12 | FallbackNodeWithMemory(std::string name); 13 | FallbackNodeWithMemory(std::string name, int reset_policy); 14 | ~FallbackNodeWithMemory(); 15 | int DrawType(); 16 | // The method that is going to be executed by the thread 17 | BT::ReturnStatus Tick(); 18 | void Halt(); 19 | private: 20 | unsigned int current_child_idx_; 21 | unsigned int reset_policy_; 22 | 23 | }; 24 | } 25 | 26 | 27 | #endif // FALLBACK_NODE_WITH_MEMORY_H 28 | -------------------------------------------------------------------------------- /include/leaf_node.h: -------------------------------------------------------------------------------- 1 | #ifndef LEAFNODE_H 2 | #define LEAFNODE_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace BT 9 | { 10 | class LeafNode : public TreeNode 11 | { 12 | protected: 13 | public: 14 | LeafNode(std::string name); 15 | ~LeafNode(); 16 | void ResetColorState(); 17 | int Depth(); 18 | }; 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /include/parallel_node.h: -------------------------------------------------------------------------------- 1 | #ifndef PARALLEL_NODE_H 2 | #define PARALLEL_NODE_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class ParallelNode : public ControlNode 9 | { 10 | public: 11 | // Constructor 12 | ParallelNode(std::string name, int threshold_M); 13 | ~ParallelNode(); 14 | int DrawType(); 15 | // The method that is going to be executed by the thread 16 | BT::ReturnStatus Tick(); 17 | void Halt(); 18 | 19 | unsigned int get_threshold_M(); 20 | void set_threshold_M(unsigned int threshold_M); 21 | 22 | private: 23 | unsigned int threshold_M_; 24 | unsigned int success_childred_num_; 25 | unsigned int failure_childred_num_; 26 | 27 | }; 28 | } 29 | #endif // PARALLEL_NODE_H 30 | -------------------------------------------------------------------------------- /include/sequence_node.h: -------------------------------------------------------------------------------- 1 | #ifndef SEQUENCENODE_H 2 | #define SEQUENCENODE_H 3 | 4 | #include 5 | 6 | namespace BT 7 | { 8 | class SequenceNode : public ControlNode 9 | { 10 | public: 11 | // Constructor 12 | SequenceNode(std::string name); 13 | ~SequenceNode(); 14 | int DrawType(); 15 | // The method that is going to be executed by the thread 16 | BT::ReturnStatus Tick(); 17 | }; 18 | } 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /include/sequence_node_with_memory.h: -------------------------------------------------------------------------------- 1 | #ifndef SEQUENCE_NODE_WITH_MEMORY_H 2 | #define SEQUENCE_NODE_WITH_MEMORY_H 3 | 4 | 5 | #include 6 | 7 | 8 | 9 | 10 | namespace BT 11 | { 12 | class SequenceNodeWithMemory : public ControlNode 13 | { 14 | public: 15 | // Constructor 16 | SequenceNodeWithMemory(std::string name); 17 | SequenceNodeWithMemory(std::string name, int reset_policy); 18 | ~SequenceNodeWithMemory(); 19 | int DrawType(); 20 | // The method that is going to be executed by the thread 21 | BT::ReturnStatus Tick(); 22 | void Halt(); 23 | private: 24 | unsigned int current_child_idx_; 25 | unsigned int reset_policy_; 26 | 27 | }; 28 | } 29 | 30 | #endif // SEQUENCE_NODE_WITH_MEMORY_H 31 | -------------------------------------------------------------------------------- /include/tick_engine.h: -------------------------------------------------------------------------------- 1 | #ifndef TICK_ENGINE_H 2 | #define TICK_ENGINE_H 3 | 4 | 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class TickEngine 12 | { 13 | private: 14 | int value_; 15 | std::mutex mutex_; 16 | std::condition_variable condition_variable_; 17 | public: 18 | TickEngine(int initial_value); 19 | ~TickEngine(); 20 | void Wait(); 21 | void Tick(); 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /include/tree_node.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIORTREECORE_TREENODE_H 2 | #define BEHAVIORTREECORE_TREENODE_H 3 | 4 | 5 | #ifndef _COLORS_ 6 | #define _COLORS_ 7 | 8 | /* FOREGROUND */ 9 | #define RST "\x1B[0m" 10 | #define KRED "\x1B[31m" 11 | #define KGRN "\x1B[32m" 12 | #define KYEL "\x1B[33m" 13 | #define KBLU "\x1B[34m" 14 | #define KMAG "\x1B[35m" 15 | #define KCYN "\x1B[36m" 16 | #define KWHT "\x1B[37m" 17 | 18 | #define FRED(x) KRED x RST 19 | #define FGRN(x) KGRN x RST 20 | #define FYEL(x) KYEL x RST 21 | #define FBLU(x) KBLU x RST 22 | #define FMAG(x) KMAG x RST 23 | #define FCYN(x) KCYN x RST 24 | #define FWHT(x) KWHT x RST 25 | 26 | #define BOLD(x) "\x1B[1m" x RST 27 | #define UNDL(x) "\x1B[4m" x RST 28 | 29 | #endif 30 | 31 | // #define DEBUG //uncomment this line if you want to print debug messages 32 | 33 | #ifdef DEBUG 34 | // #define DEBUG_STDERR(x) (std::cerr << (x)) 35 | #define DEBUG_STDOUT(str) do { std::cout << str << std::endl; } while( false ) 36 | 37 | #else 38 | #define DEBUG_STDOUT(str) 39 | #endif 40 | 41 | 42 | #include 43 | //#include 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | 53 | #include 54 | #include 55 | 56 | namespace BT 57 | { 58 | // Enumerates the possible types of a node, for drawinf we have do discriminate whoich control node it is: 59 | 60 | enum NodeType {ACTION_NODE, CONDITION_NODE, CONTROL_NODE}; 61 | enum DrawNodeType {PARALLEL, SELECTOR, SEQUENCE, SEQUENCESTAR, SELECTORSTAR, ACTION, CONDITION,DECORATOR}; 62 | // Enumerates the states every node can be in after execution during a particular 63 | // time step: 64 | // - "Success" indicates that the node has completed running during this time step; 65 | // - "Failure" indicates that the node has determined it will not be able to complete 66 | // its task; 67 | // - "Running" indicates that the node has successfully moved forward during this 68 | // time step, but the task is not yet complete; 69 | // - "Idle" indicates that the node hasn't run yet. 70 | // - "Halted" indicates that the node has been halted by its father. 71 | enum ReturnStatus {RUNNING, SUCCESS, FAILURE, IDLE, HALTED, EXIT}; 72 | 73 | // Enumerates the options for when a parallel node is considered to have failed: 74 | // - "FAIL_ON_ONE" indicates that the node will return failure as soon as one of 75 | // its children fails; 76 | // - "FAIL_ON_ALL" indicates that all of the node's children must fail before it 77 | // returns failure. 78 | enum FailurePolicy {FAIL_ON_ONE, FAIL_ON_ALL}; 79 | enum ResetPolity {ON_SUCCESS_OR_FAILURE,ON_SUCCESS, ON_FAILURE}; 80 | 81 | // Enumerates the options for when a parallel node is considered to have succeeded: 82 | // - "SUCCEED_ON_ONE" indicates that the node will return success as soon as one 83 | // of its children succeeds; 84 | // - "BT::SUCCEED_ON_ALL" indicates that all of the node's children must succeed before 85 | // it returns success. 86 | enum SuccessPolicy {SUCCEED_ON_ONE, SUCCEED_ON_ALL}; 87 | 88 | // If "BT::FAIL_ON_ONE" and "BT::SUCCEED_ON_ONE" are both active and are both trigerred in the 89 | // same time step, failure will take precedence. 90 | 91 | // Abstract base class for Behavior Tree Nodes 92 | class TreeNode 93 | { 94 | private: 95 | // Node name 96 | std::string name_; 97 | 98 | 99 | 100 | protected: 101 | // The node state that must be treated in a thread-safe way 102 | bool is_state_updated_; 103 | ReturnStatus status_; 104 | ReturnStatus color_status_; 105 | 106 | 107 | std::mutex state_mutex_; 108 | std::mutex color_state_mutex_; 109 | std::condition_variable state_condition_variable_; 110 | // Node type 111 | NodeType type_; 112 | //position and offset for horizontal positioning when drawing 113 | float x_shift_, x_pose_; 114 | 115 | public: 116 | // The thread that will execute the node 117 | std::thread thread_; 118 | 119 | // Node semaphore to simulate the tick 120 | // (and to synchronize fathers and children) 121 | TickEngine tick_engine; 122 | 123 | 124 | 125 | 126 | // The constructor and the distructor 127 | TreeNode(std::string name); 128 | ~TreeNode(); 129 | 130 | // The method that is going to be executed when the node receive a tick 131 | virtual BT::ReturnStatus Tick() = 0; 132 | 133 | // The method used to interrupt the execution of the node 134 | virtual void Halt() = 0; 135 | 136 | // The method that retrive the state of the node 137 | // (conditional waiting and mutual access) 138 | // ReturnStatus GetNodeState(); 139 | void SetNodeState(ReturnStatus new_state); 140 | void set_color_status(ReturnStatus new_color_status); 141 | 142 | // Methods used to access the node state without the 143 | // conditional waiting (only mutual access) 144 | ReturnStatus ReadState(); 145 | ReturnStatus get_color_status(); 146 | virtual int DrawType() = 0; 147 | virtual void ResetColorState() = 0; 148 | virtual int Depth() = 0; 149 | bool is_halted(); 150 | 151 | 152 | //Getters and setters 153 | void set_x_pose(float x_pose); 154 | float get_x_pose(); 155 | 156 | void set_x_shift(float x_shift); 157 | float get_x_shift(); 158 | 159 | 160 | 161 | ReturnStatus get_status(); 162 | void set_status(ReturnStatus new_status); 163 | 164 | 165 | std::string get_name(); 166 | void set_name(std::string new_name); 167 | 168 | NodeType get_type(); 169 | 170 | }; 171 | } 172 | 173 | #endif 174 | -------------------------------------------------------------------------------- /src/action_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | 17 | BT::ActionNode::ActionNode(std::string name) : LeafNode::LeafNode(name) 18 | { 19 | type_ = BT::ACTION_NODE; 20 | thread_ = std::thread(&ActionNode::WaitForTick, this); 21 | } 22 | 23 | BT::ActionNode::~ActionNode() {} 24 | 25 | 26 | void BT::ActionNode::WaitForTick() 27 | { 28 | 29 | while (true) 30 | { 31 | // Waiting for the tick to come 32 | DEBUG_STDOUT(get_name() << " WAIT FOR TICK"); 33 | 34 | tick_engine.Wait(); 35 | DEBUG_STDOUT(get_name() << " TICK RECEIVED"); 36 | 37 | // Running state 38 | set_status(BT::RUNNING); 39 | BT::ReturnStatus status = Tick(); 40 | set_status(status); 41 | } 42 | } 43 | 44 | int BT::ActionNode::DrawType() 45 | { 46 | return BT::ACTION; 47 | } 48 | -------------------------------------------------------------------------------- /src/behavior_tree.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | #include 15 | 16 | 17 | 18 | void Execute(BT::ControlNode* root, int TickPeriod_milliseconds) 19 | { 20 | std::cout << "Start Drawing!" << std::endl; 21 | // Starts in another thread the drawing of the BT 22 | std::thread t(&drawTree, root); 23 | 24 | root->ResetColorState(); 25 | 26 | while (true) 27 | { 28 | DEBUG_STDOUT("Ticking the root node !"); 29 | 30 | // Ticking the root node 31 | root->Tick(); 32 | // Printing its state 33 | 34 | if (root->get_status() != BT::RUNNING) 35 | { 36 | // when the root returns a status it resets the colors of the tree 37 | root->ResetColorState(); 38 | } 39 | std::this_thread::sleep_for(std::chrono::milliseconds(TickPeriod_milliseconds)); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/condition_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::ConditionNode::ConditionNode(std::string name) : LeafNode::LeafNode(name) 17 | { 18 | type_ = BT::CONDITION_NODE; 19 | } 20 | 21 | BT::ConditionNode::~ConditionNode() {} 22 | 23 | void BT::ConditionNode::Halt() {} 24 | 25 | int BT::ConditionNode::DrawType() 26 | { 27 | return BT::CONDITION; 28 | } 29 | -------------------------------------------------------------------------------- /src/control_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | BT::ControlNode::ControlNode(std::string name) : TreeNode::TreeNode(name) 19 | { 20 | type_ = BT::CONTROL_NODE; 21 | 22 | // TODO(...) In case it is desired to set to idle remove the ReturnStatus 23 | // type in order to set the member variable 24 | // ReturnStatus child_i_status_ = BT::IDLE; // commented out as unused 25 | } 26 | 27 | BT::ControlNode::~ControlNode() {} 28 | 29 | void BT::ControlNode::AddChild(TreeNode* child) 30 | { 31 | // // Checking if the child is not already present 32 | // for (unsigned int i=0; iget_name() + "' is already a '" + get_name() + "' child."); 37 | // } 38 | // } 39 | 40 | children_nodes_.push_back(child); 41 | children_states_.push_back(BT::IDLE); 42 | } 43 | 44 | unsigned int BT::ControlNode::GetChildrenNumber() 45 | { 46 | return children_nodes_.size(); 47 | } 48 | 49 | void BT::ControlNode::Halt() 50 | { 51 | DEBUG_STDOUT("HALTING: "<< get_name()); 52 | HaltChildren(0); 53 | set_status(BT::HALTED); 54 | } 55 | 56 | std::vector BT::ControlNode::GetChildren() 57 | { 58 | return children_nodes_; 59 | } 60 | 61 | void BT::ControlNode::ResetColorState() 62 | { 63 | set_color_status(BT::IDLE); 64 | for (unsigned int i = 0; i < children_nodes_.size(); i++) 65 | { 66 | children_nodes_[i]->ResetColorState(); 67 | } 68 | } 69 | 70 | void BT::ControlNode::HaltChildren(int i) 71 | { 72 | for (unsigned int j=i; j < children_nodes_.size(); j++) 73 | { 74 | if (children_nodes_[j]->get_type() == BT::CONDITION_NODE) 75 | { 76 | children_nodes_[i]->ResetColorState(); 77 | } 78 | else 79 | { 80 | if (children_nodes_[j]->get_status() == BT::RUNNING) 81 | { 82 | DEBUG_STDOUT("SENDING HALT TO CHILD " << children_nodes_[j]-> get_name()); 83 | children_nodes_[j]->Halt(); 84 | } 85 | else 86 | { 87 | DEBUG_STDOUT("NO NEED TO HALT " << children_nodes_[j]-> get_name() 88 | << "STATUS" << children_nodes_[j]->get_status()); 89 | } 90 | } 91 | } 92 | } 93 | 94 | int BT::ControlNode::Depth() 95 | { 96 | int depMax = 0; 97 | int dep = 0; 98 | for (unsigned int i = 0; i < children_nodes_.size(); i++) 99 | { 100 | dep = (children_nodes_[i]->Depth()); 101 | if (dep > depMax) 102 | { 103 | depMax = dep; 104 | } 105 | } 106 | return 1 + depMax; 107 | } 108 | 109 | -------------------------------------------------------------------------------- /src/decorator_negation_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | BT::DecoratorNegationNode::DecoratorNegationNode(std::string name) : ControlNode::ControlNode(name) 5 | { 6 | // thread_ start 7 | thread_ = boost::thread(&DecoratorNegationNode::Exec, this); 8 | } 9 | 10 | BT::DecoratorNegationNode::~DecoratorNegationNode() {} 11 | 12 | void BT::DecoratorNegationNode::Exec() 13 | { 14 | 15 | 16 | // Waiting for the first tick to come 17 | tick_engine.wait(); 18 | 19 | // Vector size initialization 20 | N_of_children_ = children_nodes_.size(); 21 | 22 | // Simulating a tick for myself 23 | tick_engine.tick(); 24 | 25 | while(true) 26 | { 27 | // Waiting for a tick to come 28 | tick_engine.wait(); 29 | 30 | if(ReadState() == BT::EXIT) 31 | { 32 | // The behavior tree is going to be destroied 33 | return; 34 | } 35 | 36 | // Checking if i was halted 37 | if (ReadState() != BT::HALTED) 38 | { 39 | // If not, the children can be ticked 40 | std::cout << get_name() << " ticked, ticking children..." << std::endl; 41 | 42 | 43 | 44 | 45 | if (children_nodes_[0]->get_type() == BT::ACTION_NODE) 46 | { 47 | // 1) if it's an action: 48 | // 1.1) read its state; 49 | ReturnStatus ActionState = children_nodes_[0]->ReadState(); 50 | 51 | if (ActionState == BT::IDLE) 52 | { 53 | // 1.2) if it's "Idle": 54 | // 1.2.1) ticking it; 55 | children_nodes_[0]->tick_engine.tick(); 56 | 57 | // 1.2.2) retrive its state as soon as it is available; 58 | children_states_[0] = children_nodes_[0]->GetNodeState(); 59 | } 60 | else if (ActionState == BT::RUNNING) 61 | { 62 | // 1.3) if it's "Running": 63 | // 1.3.1) saving "Running" 64 | children_states_[0] = BT::RUNNING; 65 | } 66 | else 67 | { 68 | // 1.4) if it's "Success" of "Failure" (it can't be "Halted"!): 69 | // 1.2.1) ticking it; 70 | children_nodes_[0]->tick_engine.tick(); 71 | 72 | // 1.2.2) saving the read state; 73 | children_states_[0] = ActionState; 74 | } 75 | } 76 | else 77 | { 78 | // 2) if it's not an action: 79 | // 2.1) ticking it; 80 | children_nodes_[0]->tick_engine.tick(); 81 | 82 | // 2.2) retrive its state as soon as it is available; 83 | children_states_[0] = children_nodes_[0]->GetNodeState(); 84 | } 85 | 86 | // 3) if the child state is a success: 87 | if(children_states_[0] == BT::SUCCESS) 88 | { 89 | // 3.1) the node state is equal to failure since I am negating the status 90 | SetNodeState(BT::FAILURE); 91 | 92 | // 3.2) resetting the state; 93 | WriteState(BT::IDLE); 94 | 95 | std::cout << get_name() << " returning " << BT::FAILURE << "!" << std::endl; 96 | } 97 | else if(children_states_[0] == BT::FAILURE) 98 | { 99 | // 4.1) the node state is equal to success since I am negating the status 100 | SetNodeState(BT::SUCCESS); 101 | 102 | // 4.2) state reset; 103 | WriteState(BT::IDLE); 104 | 105 | std::cout << get_name() << " returning " << BT::SUCCESS << "!" << std::endl; 106 | 107 | } else 108 | // 5) if the child state is running 109 | { 110 | // 5.1) the node state is equal to running 111 | SetNodeState(BT::RUNNING); 112 | 113 | // 5.2) state reset; 114 | WriteState(BT::IDLE); 115 | } 116 | 117 | } 118 | else 119 | { 120 | // If it was halted, all the "busy" children must be halted too 121 | std::cout << get_name() << " halted! Halting all the children..." << std::endl; 122 | 123 | if (children_nodes_[0]->get_type() != BT::ACTION_NODE && children_states_[0] == BT::RUNNING) 124 | { 125 | // if the control node was running: 126 | // halting it; 127 | children_nodes_[0]->Halt(); 128 | 129 | // sync with it (it's waiting on the semaphore); 130 | children_nodes_[0]->tick_engine.tick(); 131 | 132 | std::cout << get_name() << " halting child " << "!" << std::endl; 133 | } 134 | else if (children_nodes_[0]->get_type() == BT::ACTION_NODE && children_nodes_[0]->ReadState() == BT::RUNNING) 135 | { 136 | std::cout << get_name() << " trying halting child " << "..." << std::endl; 137 | 138 | // if it's a action node that hasn't finished its job: 139 | // trying to halt it: 140 | if (children_nodes_[0]->Halt() == false) 141 | { 142 | // this means that, before this node could set its child state 143 | // to "Halted", the child had already written the action outcome; 144 | // sync with him ignoring its state; 145 | children_nodes_[0]->tick_engine.tick(); 146 | 147 | std::cout << get_name() << " halting of child " << " failed!" << std::endl; 148 | } 149 | 150 | std::cout << get_name() << " halting of child " << " succedeed!" << std::endl; 151 | } 152 | else if (children_nodes_[0]->get_type() == BT::ACTION_NODE && children_nodes_[0]->ReadState() != BT::IDLE) 153 | { 154 | // if it's a action node that has finished its job: 155 | // ticking it without saving its returning state; 156 | children_nodes_[0]->tick_engine.tick(); 157 | } 158 | 159 | // updating its vector cell 160 | children_states_[0] = BT::IDLE; 161 | 162 | 163 | // Resetting the node state 164 | WriteState(BT::IDLE); 165 | } 166 | } 167 | } 168 | 169 | int BT::DecoratorNegationNode::DrawType() 170 | { 171 | // Lock acquistion 172 | 173 | return BT::DECORATOR; 174 | } 175 | 176 | 177 | void BT::DecoratorNegationNode::AddChild(BT::TreeNode* child) 178 | { 179 | // Checking if the child is not already present 180 | 181 | if (children_nodes_.size() > 0) 182 | { 183 | throw BehaviorTreeException("Decorators can have only one child"); 184 | } 185 | 186 | 187 | children_nodes_.push_back(child); 188 | children_states_.push_back(BT::IDLE); 189 | } 190 | -------------------------------------------------------------------------------- /src/decorator_retry_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | BT::DecoratorRetryNode::DecoratorRetryNode(std::string name, unsigned int NTries) : ControlNode::ControlNode(name) 5 | { 6 | // thread_ start 7 | NTries_ = NTries; 8 | thread_ = boost::thread(&DecoratorRetryNode::Exec, this); 9 | } 10 | 11 | BT::DecoratorRetryNode::~DecoratorRetryNode() {} 12 | 13 | void BT::DecoratorRetryNode::Exec() 14 | { 15 | int i; 16 | 17 | // Waiting for the first tick to come 18 | tick_engine.wait(); 19 | 20 | // Vector size initialization 21 | N_of_children_ = children_nodes_.size(); 22 | 23 | // Simulating a tick for myself 24 | tick_engine.tick(); 25 | 26 | while(true) 27 | { 28 | // Waiting for a tick to come 29 | tick_engine.wait(); 30 | 31 | if(ReadState() == BT::EXIT) 32 | { 33 | // The behavior tree is going to be destroied 34 | return; 35 | } 36 | 37 | // Checking if i was halted 38 | if (ReadState() != BT::HALTED) 39 | { 40 | // If not, the children can be ticked 41 | std::cout << get_name() << " ticked, ticking children..." << std::endl; 42 | 43 | TryIndx_ = 0; 44 | // For each child: 45 | //for (i = 0; iget_type() == BT::ACTION_NODE) 48 | { 49 | // 1) if it's an action: 50 | // 1.1) read its state; 51 | ReturnStatus ActionState = children_nodes_[0]->ReadState(); 52 | 53 | if (ActionState == BT::IDLE) 54 | { 55 | // 1.2) if it's "Idle": 56 | // 1.2.1) ticking it; 57 | children_nodes_[0]->tick_engine.tick(); 58 | 59 | // 1.2.2) retrive its state as soon as it is available; 60 | children_states_[0] = children_nodes_[0]->GetNodeState(); 61 | } 62 | else if (ActionState == BT::RUNNING) 63 | { 64 | // 1.3) if it's "Running": 65 | // 1.3.1) saving "Running" 66 | children_states_[0] = BT::RUNNING; 67 | } 68 | else 69 | { 70 | // 1.4) if it's "Success" of "Failure" (it can't be "Halted"!): 71 | // 1.2.1) ticking it; 72 | children_nodes_[0]->tick_engine.tick(); 73 | 74 | // 1.2.2) saving the read state; 75 | children_states_[0] = ActionState; 76 | } 77 | } 78 | else 79 | { 80 | // 2) if it's not an action: 81 | // 2.1) ticking it; 82 | children_nodes_[0]->tick_engine.tick(); 83 | 84 | // 2.2) retrive its state as soon as it is available; 85 | children_states_[0] = children_nodes_[0]->GetNodeState(); 86 | } 87 | 88 | // 3) if the child state is not a success: 89 | if(children_states_[0] == BT::SUCCESS) 90 | { 91 | SetNodeState(BT::SUCCESS); 92 | 93 | // 4.2) resetting the state; 94 | WriteState(BT::IDLE); 95 | 96 | std::cout << get_name() << " returning " << BT::SUCCESS << "!" << std::endl; 97 | } 98 | else 99 | { 100 | if(children_states_[0] == BT::FAILURE) 101 | { 102 | children_nodes_[0]->ResetColorState(); 103 | TryIndx_++; 104 | 105 | } 106 | 107 | if(children_states_[0] == BT::FAILURE && TryIndx_ < NTries_) 108 | { 109 | // 3.1) the node state is equal to running since I am rerunning the child 110 | SetNodeState(BT::RUNNING); 111 | // 3.2) state reset; 112 | WriteState(BT::IDLE); 113 | } 114 | else 115 | { 116 | SetNodeState(children_states_[0]); 117 | // 3.2) state reset; 118 | WriteState(BT::IDLE); 119 | std::cout << get_name() << " returning " << children_states_[0] << "!" << std::endl; 120 | 121 | } 122 | } 123 | } 124 | 125 | } 126 | else 127 | { 128 | // If it was halted, all the "busy" children must be halted too 129 | std::cout << get_name() << " halted! Halting all the children..." << std::endl; 130 | 131 | if (children_nodes_[0]->get_type() != BT::ACTION_NODE && children_states_[0] == BT::RUNNING) 132 | { 133 | // if the control node was running: 134 | // halting it; 135 | children_nodes_[0]->Halt(); 136 | 137 | // sync with it (it's waiting on the semaphore); 138 | children_nodes_[0]->tick_engine.tick(); 139 | 140 | std::cout << get_name() << " halting child " << "!" << std::endl; 141 | } 142 | else if (children_nodes_[0]->get_type() == BT::ACTION_NODE && children_nodes_[0]->ReadState() == BT::RUNNING) 143 | { 144 | std::cout << get_name() << " trying halting child " << "..." << std::endl; 145 | 146 | // if it's a action node that hasn't finished its job: 147 | // trying to halt it: 148 | if (children_nodes_[0]->Halt() == false) 149 | { 150 | // this means that, before this node could set its child state 151 | // to "Halted", the child had already written the action outcome; 152 | // sync with him ignoring its state; 153 | children_nodes_[0]->tick_engine.tick(); 154 | 155 | std::cout << get_name() << " halting of child " << " failed!" << std::endl; 156 | } 157 | 158 | std::cout << get_name() << " halting of child " << " succedeed!" << std::endl; 159 | } 160 | else if (children_nodes_[0]->get_type() == BT::ACTION_NODE && children_nodes_[0]->ReadState() != BT::IDLE) 161 | { 162 | // if it's a action node that has finished its job: 163 | // ticking it without saving its returning state; 164 | children_nodes_[0]->tick_engine.tick(); 165 | } 166 | 167 | // updating its vector cell 168 | children_states_[0] = BT::IDLE; 169 | 170 | 171 | // Resetting the node state 172 | WriteState(BT::IDLE); 173 | } 174 | } 175 | } 176 | 177 | int BT::DecoratorRetryNode::DrawType() 178 | { 179 | // Lock acquistion 180 | 181 | return BT::DECORATOR; 182 | } 183 | 184 | -------------------------------------------------------------------------------- /src/draw.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | const float DEG2RAD = 3.14159/180.0; 22 | 23 | 24 | BT::ControlNode* tree; 25 | bool init = false; 26 | 27 | 28 | void * font_array[3] = {GLUT_BITMAP_8_BY_13, GLUT_BITMAP_8_BY_13, GLUT_BITMAP_8_BY_13}; 29 | void * font = font_array[0]; 30 | float additional_spacing_array[10]; 31 | bool is_number_pressed_array[10]; 32 | unsigned char number_char[4] = {'0', '1', '2', '3'}; 33 | 34 | 35 | float x = 0.0; 36 | float y = 0.4; 37 | float x_offset = 0.01; 38 | float y_offset = 0.15; 39 | float r_color = 1; 40 | float g_color = 1; 41 | float b_color = 1; 42 | GLfloat x_space = 0.06; 43 | 44 | int depth; 45 | 46 | double zoom = 1.0f; 47 | 48 | float fraction = 0.1f; 49 | float zoom_fraction = 0.1f; 50 | 51 | 52 | 53 | 54 | void drawEllipse(float xpos, float ypos, float xradius, float yradius) 55 | { 56 | glBegin(GL_LINE_LOOP); 57 | 58 | for (int i=0; i < 359; i++) 59 | { 60 | // convert degrees into radians 61 | float degInRad = i*DEG2RAD; 62 | glVertex2d(xpos+cos(degInRad)*xradius, ypos + sin(degInRad)*yradius); 63 | } 64 | glEnd(); 65 | } 66 | 67 | void drawString(void * font, const char *string, float x, float y, float z) 68 | { 69 | renderBitmapString(x, y, font, string); 70 | } 71 | 72 | int compute_node_lines(const char *string) 73 | { 74 | const char *c; 75 | int i = 0; 76 | int new_line_num = 1; 77 | glRasterPos2f(x, y); 78 | for (c=string; *c != '\0'; c++) 79 | { 80 | if ((*c == '\n') || ((*c == ' ' && i > 6) || i > 9)) 81 | { 82 | new_line_num++; 83 | i = 0; 84 | continue; 85 | } 86 | i++; 87 | } 88 | return new_line_num; 89 | } 90 | 91 | int compute_max_width(const char *string) 92 | { 93 | const char *current_char; 94 | int current_line_width = 0; 95 | int max_width = 0; 96 | 97 | glRasterPos2f(x, y); 98 | for (current_char = string; *current_char != '\0'; current_char++) 99 | { 100 | if ((*current_char == '\n') || ((*current_char == ' ' && current_line_width > 6) || current_line_width > 9)) 101 | { 102 | if (current_line_width > max_width) 103 | { 104 | max_width = current_line_width; 105 | } 106 | current_line_width = 0; 107 | continue; 108 | } 109 | current_line_width++; 110 | } 111 | 112 | if (max_width == 0) // if the lable fits in a single line 113 | { 114 | max_width = current_line_width; 115 | } 116 | return max_width; 117 | } 118 | 119 | void renderBitmapString(float x, float y, void *font, const char *string) 120 | { 121 | const char *c; 122 | int i = 0; 123 | int new_line_num = 0; 124 | glRasterPos2f(x, y); 125 | for (c=string; *c != '\0'; c++) 126 | { 127 | if ((*c == '\n') || ((*c == ' ' && i > 6) || i > 9)) 128 | { 129 | new_line_num++; 130 | glRasterPos2f(x, y - 0.025*(new_line_num)); 131 | i = 0; 132 | continue; 133 | } 134 | i++; 135 | glutBitmapCharacter(font, *c); 136 | } 137 | } 138 | 139 | 140 | 141 | void draw_node(float x, float y, int node_type, const char *leafName, int status) 142 | { 143 | float NODE_WIDTH = 0.04; 144 | float NODE_HEIGHT = 0.02; 145 | switch (node_type) 146 | { 147 | case BT::SELECTORSTAR: 148 | drawString(font, "?*", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 149 | break; 150 | case BT::SEQUENCESTAR: 151 | drawString(font, ">*", (x + NODE_WIDTH/2 -0.0051), (y - NODE_HEIGHT/2), 0); 152 | break; 153 | case BT::SELECTOR: 154 | drawString(font, "?", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 155 | break; 156 | case BT::SEQUENCE: 157 | drawString(font, ">", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 158 | break; 159 | case BT::PARALLEL: 160 | drawString(font, "=", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 161 | break; 162 | case BT::DECORATOR: 163 | drawString(font, "D", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 164 | break; 165 | case BT::ACTION: 166 | { 167 | NODE_HEIGHT = 0.02*(compute_node_lines(leafName)); 168 | std::string st(leafName, 0, 15); 169 | NODE_WIDTH = 0.02*compute_max_width(leafName); 170 | // for (unsigned int i = 0; i < st.size(); i++) 171 | // NODE_WIDTH += 0.01; 172 | } 173 | renderBitmapString((x + 0.015), (y - 0.01), font, leafName); 174 | // glColor3f(0.2, 1.0, 0.2); 175 | break; 176 | case BT::CONDITION: 177 | { 178 | NODE_HEIGHT = 0.02*compute_node_lines(leafName); 179 | std::string st(leafName, 0, 15); 180 | NODE_WIDTH = 0.02*compute_max_width(leafName); 181 | } 182 | renderBitmapString((x + 2*0.015), (y - 0.01), font, leafName); 183 | break; 184 | default: 185 | break; 186 | } 187 | 188 | switch (status) 189 | { 190 | case BT::RUNNING: 191 | glColor3f(0.8, 0.8, 0.8); 192 | break; 193 | case BT::SUCCESS: 194 | glColor3f(0.0, 1.0, 0.0); 195 | break; 196 | case BT::FAILURE: 197 | glColor3f(1.0, 0.0, 0.0); 198 | break; 199 | case BT::IDLE: 200 | glColor3f(0.0, 0.0, 0.0); 201 | break; 202 | case BT::HALTED: 203 | glColor3f(0.0, 0.0, 0.0); 204 | break; 205 | default: 206 | break; 207 | } 208 | 209 | switch (node_type) 210 | { 211 | case BT::CONDITION: 212 | // drawEllipse(x,y,NODE_WIDTH,NODE_HEIGHT); 213 | // drawEllipse(x,y,0.1,0.021); 214 | glBegin(GL_LINE_LOOP); 215 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT - 0.015)); 216 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + 0.02)); 217 | glVertex2f((GLfloat) (x), (GLfloat) (y + 0.02)); 218 | glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT - 0.015)); 219 | glColor3f(0.0, 0.0, 0.0); 220 | glEnd(); 221 | break; 222 | 223 | break; 224 | case BT::ACTION: 225 | 226 | glBegin(GL_LINE_LOOP); 227 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT - 0.015)); 228 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + 0.02)); 229 | glVertex2f((GLfloat) (x), (GLfloat) (y + 0.02)); 230 | glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT - 0.015)); 231 | glColor3f(0.0, 0.0, 0.0); 232 | glEnd(); 233 | break; 234 | 235 | default: 236 | glBegin(GL_LINE_LOOP); 237 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT)); 238 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + NODE_HEIGHT)); 239 | glVertex2f((GLfloat) (x), (GLfloat) (y + NODE_HEIGHT)); 240 | glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT)); 241 | glColor3f(0.0, 0.0, 0.0); 242 | glEnd(); 243 | break; 244 | } 245 | } 246 | 247 | // draw the edge connecting one node to the other 248 | void draw_edge(GLfloat parent_x, GLfloat parent_y, 249 | GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size) 250 | { 251 | glLineWidth(1.5); 252 | glColor3f(0.0, 0.0, 0.0); 253 | // GLfloat bottom_spacing = 0.1; // commented-out as unused variable 254 | GLfloat above_spacing = 0.04; 255 | 256 | glBegin(GL_LINES); 257 | glVertex3f(parent_x, parent_y - parent_size, 0); 258 | glVertex3f(parent_x, child_y + child_size + above_spacing, 0); 259 | glEnd(); 260 | 261 | glBegin(GL_LINES); 262 | glVertex3f(parent_x, child_y + child_size + above_spacing, 0); 263 | glVertex3f(child_x, child_y + child_size + above_spacing, 0); 264 | glEnd(); 265 | 266 | glBegin(GL_LINES); 267 | glVertex3f(child_x, child_y + child_size + above_spacing, 0); 268 | glVertex3f(child_x, child_y+child_size, 0); 269 | glEnd(); 270 | } 271 | 272 | // draw the edge connecting one node to the other 273 | void draw_straight_edge(GLfloat parent_x, GLfloat parent_y, 274 | GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size) 275 | { 276 | glLineWidth(1.5); 277 | glColor3f(0.0, 0.0, 0.0); 278 | 279 | glBegin(GL_LINES); 280 | glVertex3f(parent_x, parent_y-parent_size, 0.0); 281 | glVertex3f(child_x, child_y+child_size, 0); 282 | glEnd(); 283 | } 284 | 285 | // Keyboard callback function ( called on keyboard event handling ) 286 | void keyboard(unsigned char key, int x, int y) 287 | { 288 | for (int i = 1; i < 4; i++) 289 | { 290 | if (key == number_char[i]) 291 | { 292 | is_number_pressed_array[i] = true; 293 | } 294 | else 295 | { 296 | is_number_pressed_array[i] = false; 297 | } 298 | } 299 | } 300 | 301 | void keyboard_release(unsigned char key, int x, int y) 302 | { 303 | for (int i = 1; i < 4; i++) 304 | { 305 | if (key == number_char[i]) 306 | { 307 | is_number_pressed_array[i] = false; 308 | } 309 | } 310 | } 311 | 312 | 313 | void drawCircle(float radius) 314 | { 315 | glBegin(GL_LINE_LOOP); 316 | 317 | for (int i=0; i<= 360; i++) 318 | { 319 | float degInRad = i*3.14142/180; 320 | glVertex2f(cos(degInRad)*radius, sin(degInRad)*radius); 321 | } 322 | glEnd(); 323 | } 324 | 325 | 326 | void updateTree(BT::TreeNode* tree, GLfloat x_pos, GLfloat y_pos, GLfloat y_offset, int depth ) 327 | { 328 | BT::ControlNode* d = dynamic_cast (tree); 329 | if (d == NULL) 330 | { 331 | // if it is a leaf node, draw it 332 | draw_node(x_pos , (GLfloat) y_pos, tree->DrawType(), tree->get_name().c_str(), tree->get_color_status()); 333 | } 334 | else 335 | { 336 | // if it is a control flow node, draw it and its children 337 | draw_node((GLfloat) x_pos, (GLfloat) y_pos, tree->DrawType(), 338 | tree->get_name().c_str(), tree->get_color_status()); 339 | std::vector children = d->GetChildren(); 340 | int M = d->GetChildrenNumber(); 341 | std::vector children_x_end; 342 | std::vector children_x_middle_relative; 343 | 344 | GLfloat max_x_end = 0; 345 | // GLfloat max_x_start = 0; // commented out as unused variable 346 | GLfloat current_x_end = 0; 347 | 348 | for (int i = 0; i < M; i++) 349 | { 350 | if (children[i]->DrawType() != BT::ACTION && children[i]->DrawType() != BT::CONDITION) 351 | { 352 | current_x_end = 0.04; 353 | children_x_middle_relative.push_back(0.02); 354 | } 355 | else 356 | { 357 | current_x_end = 0.02*compute_max_width(children[i]->get_name().c_str()); 358 | children_x_middle_relative.push_back(current_x_end/2); 359 | } 360 | 361 | if (i < M-1) 362 | { 363 | max_x_end = max_x_end + current_x_end + x_space + additional_spacing_array[depth]; 364 | } 365 | else 366 | { 367 | max_x_end = max_x_end + current_x_end; 368 | } 369 | children_x_end.push_back(max_x_end); 370 | } 371 | 372 | // GLfloat x_min = 0.0; // commented-out as unused variable 373 | // GLfloat x_max = 0.0; // commented-out as unused variable 374 | GLfloat x_shift = x_pos - max_x_end/2; 375 | // GLfloat x_shift_new = 0; // commented-out as unused variable 376 | 377 | for (int i = 0; i < M; i++) 378 | { 379 | if (i > 0) 380 | { 381 | updateTree(children[i], x_shift + children_x_end.at(i - 1) , y_pos - y_offset , y_offset, depth + 1); 382 | draw_edge(x_pos + 0.015, y_pos, 0.02, 383 | x_shift + children_x_end.at(i-1) + children_x_middle_relative.at(i), 384 | y_pos - y_offset, 0.02); 385 | } 386 | else 387 | { 388 | updateTree(children[i], x_shift , y_pos - y_offset , y_offset, depth + 1); 389 | draw_edge(x_pos + 0.015, y_pos, 0.02, 390 | x_shift + children_x_middle_relative.at(i), y_pos - y_offset, 0.02); 391 | } 392 | } 393 | } 394 | } 395 | 396 | 397 | 398 | 399 | 400 | void display() 401 | { 402 | glClearColor(r_color, g_color, b_color, 0.1); // clear the draw buffer . 403 | glClear(GL_COLOR_BUFFER_BIT); // Erase everything 404 | updateTree(tree, x , y, y_offset, 1); 405 | glutSwapBuffers(); 406 | glutPostRedisplay(); 407 | } 408 | 409 | 410 | void processSpecialKeys(int key, int xx, int yy) 411 | { 412 | switch (key) 413 | { 414 | case GLUT_KEY_UP : 415 | y += fraction; 416 | break; 417 | case GLUT_KEY_DOWN : 418 | if (y >= 0) 419 | { 420 | y -= fraction; 421 | } 422 | break; 423 | case GLUT_KEY_LEFT: 424 | x -= fraction; 425 | break; 426 | case GLUT_KEY_RIGHT: 427 | x += fraction; 428 | break; 429 | case GLUT_KEY_PAGE_UP: 430 | for (int i = 1; i < 10; i++) 431 | { 432 | if (is_number_pressed_array[i]) 433 | { 434 | additional_spacing_array[i] += fraction; 435 | } 436 | } 437 | break; 438 | case GLUT_KEY_PAGE_DOWN: 439 | for (int i = 1; i < 10; i++) 440 | { 441 | if (is_number_pressed_array[i] && additional_spacing_array[i] >= 0 ) 442 | { 443 | additional_spacing_array[i] -= fraction; 444 | } 445 | } 446 | break; 447 | case GLUT_KEY_F1: 448 | if (r_color < 1) r_color += fraction; 449 | break; 450 | case GLUT_KEY_F2: 451 | if (r_color > 0) r_color -= fraction; 452 | break; 453 | case GLUT_KEY_F3: 454 | if (g_color < 1) g_color += fraction; 455 | break; 456 | case GLUT_KEY_F4: 457 | if (g_color > 0) g_color -= fraction; 458 | break; 459 | case GLUT_KEY_F5: 460 | if (b_color < 1) b_color += fraction; 461 | break; 462 | case GLUT_KEY_F6: 463 | if (b_color > 0) b_color -= fraction; 464 | break; 465 | case GLUT_KEY_HOME: 466 | if (zoom < 1.0f) 467 | { 468 | glScalef(1.0f + zoom_fraction, 1.0f + zoom_fraction, 1.0f); 469 | zoom += zoom_fraction; 470 | } 471 | else 472 | { 473 | glScalef(1.0f, 1.0f, 1.0f); 474 | } 475 | break; 476 | case GLUT_KEY_END: 477 | glScalef(1.0f - zoom_fraction, 1.0f - zoom_fraction, 1.0f); 478 | zoom -= zoom_fraction; 479 | break; 480 | } 481 | } 482 | 483 | void drawTree(BT::ControlNode* tree_) 484 | { 485 | /***************************BT VISUALIZATION****************************/ 486 | int argc = 1; 487 | char *argv[1] = {const_cast("")}; 488 | 489 | if (!init) 490 | { 491 | XInitThreads(); 492 | glutInit(&argc, argv); 493 | init = true; 494 | glutInitDisplayMode(GLUT_DEPTH | GLUT_RGBA | GLUT_DOUBLE | GLUT_MULTISAMPLE ); // Antialiasing 495 | glEnable(GL_MULTISAMPLE); 496 | } 497 | tree = tree_; 498 | depth = tree->Depth(); 499 | 500 | glutInitWindowSize(1024, 860); 501 | 502 | glutCreateWindow("Behavior Tree"); // Create a window 503 | 504 | glClearColor(0, 0.71, 0.00, 0.1); 505 | glutDisplayFunc(display); // Register display callback 506 | 507 | glutKeyboardFunc(keyboard); // Register keyboard presscallback 508 | glutKeyboardUpFunc(keyboard_release); // Register keyboard release callback 509 | 510 | glutSpecialFunc(processSpecialKeys); // Register keyboard arrow callback 511 | 512 | glutMainLoop(); // Enter main event loop 513 | 514 | /***************************ENDOF BT VISUALIZATION ****************************/ 515 | } 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | -------------------------------------------------------------------------------- /src/example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | 6 | class MyCondition : public BT::ConditionNode 7 | { 8 | public: 9 | MyCondition(std::string name); 10 | ~MyCondition(); 11 | BT::ReturnStatus Tick(); 12 | }; 13 | 14 | MyCondition::MyCondition(std::string name) : BT::ConditionNode::ConditionNode(name) 15 | { 16 | 17 | } 18 | 19 | BT::ReturnStatus MyCondition::Tick() 20 | { 21 | std::cout << "The Condition is true" << std::endl; 22 | 23 | return BT::SUCCESS; 24 | } 25 | 26 | 27 | class MyAction : public BT::ActionNode 28 | { 29 | public: 30 | MyAction(std::string name); 31 | ~MyAction(); 32 | BT::ReturnStatus Tick(); 33 | void Halt(); 34 | }; 35 | 36 | MyAction::MyAction(std::string name) : ActionNode::ActionNode(name) 37 | { 38 | 39 | } 40 | 41 | 42 | BT::ReturnStatus MyAction::Tick() 43 | { 44 | std::cout << "The Action is doing some operations" << std::endl; 45 | std::this_thread::sleep_for(std::chrono::seconds(5)); 46 | if (is_halted()) 47 | { 48 | return BT::HALTED; 49 | } 50 | 51 | std::cout << "The Action is doing some others operations" << std::endl; 52 | std::this_thread::sleep_for(std::chrono::seconds(5)); 53 | if (is_halted()) 54 | { 55 | return BT::HALTED; 56 | } 57 | 58 | std::cout << "The Action is doing more operations" << std::endl; 59 | std::this_thread::sleep_for(std::chrono::seconds(5)); 60 | if (is_halted()) 61 | { 62 | return BT::HALTED; 63 | } 64 | 65 | std::cout << "The Action has succeeded" << std::endl; 66 | return BT::SUCCESS; 67 | } 68 | 69 | void MyAction::Halt() 70 | { 71 | 72 | } 73 | 74 | 75 | int main(int argc, char *argv[]) 76 | { 77 | 78 | BT::SequenceNode* seq = new BT::SequenceNode("Sequence"); 79 | MyCondition* my_con_1 = new MyCondition("Condition"); 80 | MyAction* my_act_1 = new MyAction("Action"); 81 | int tick_time_milliseconds = 1000; 82 | 83 | seq->AddChild(my_con_1); 84 | seq->AddChild(my_act_1); 85 | 86 | Execute(seq, tick_time_milliseconds); 87 | 88 | return 0; 89 | } 90 | -------------------------------------------------------------------------------- /src/exceptions.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::BehaviorTreeException::BehaviorTreeException(const std::string Message) 17 | { 18 | this->Message = std::string("BehaviorTreeException: " + Message).c_str(); 19 | } 20 | -------------------------------------------------------------------------------- /src/fallback_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::FallbackNode::FallbackNode(std::string name) : ControlNode::ControlNode(name) {} 17 | 18 | BT::FallbackNode::~FallbackNode() {} 19 | 20 | BT::ReturnStatus BT::FallbackNode::Tick() 21 | { 22 | { 23 | // gets the number of children. The number could change if, at runtime, one edits the tree. 24 | N_of_children_ = children_nodes_.size(); 25 | 26 | // Routing the ticks according to the fallback node's logic: 27 | 28 | for (unsigned int i = 0; i < N_of_children_; i++) 29 | { 30 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 31 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 32 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 33 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 34 | */ 35 | if (children_nodes_[i]->get_type() == BT::ACTION_NODE) 36 | { 37 | // 1) If the child i is an action, read its state. 38 | child_i_status_ = children_nodes_[i]->get_status(); 39 | 40 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 41 | { 42 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 43 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[i]->get_name()); 44 | children_nodes_[i]->tick_engine.Tick(); 45 | 46 | // waits for the tick to arrive to the child 47 | do 48 | { 49 | child_i_status_ = children_nodes_[i]->get_status(); 50 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 51 | } 52 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 53 | && child_i_status_ != BT::FAILURE); 54 | } 55 | } 56 | else 57 | { 58 | // 2) if it's not an action: 59 | // Send the tick and wait for the response; 60 | child_i_status_ = children_nodes_[i]->Tick(); 61 | children_nodes_[i]->set_status(child_i_status_); 62 | 63 | } 64 | // Ponderate on which status to send to the parent 65 | if (child_i_status_ != BT::FAILURE) 66 | { 67 | if (child_i_status_ == BT::SUCCESS) 68 | { 69 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned success. 70 | } 71 | // If the child status is not failure, halt the next children and return the status to your parent. 72 | DEBUG_STDOUT(get_name() << " is HALTING children from " << (i+1)); 73 | HaltChildren(i+1); 74 | set_status(child_i_status_); 75 | return child_i_status_; 76 | } 77 | else 78 | { 79 | // the child returned failure. 80 | children_nodes_[i]->set_status(BT::IDLE); 81 | if (i == N_of_children_ - 1) 82 | { 83 | // If the child status is failure, and it is the last child to be ticked, 84 | // then the sequence has failed. 85 | set_status(BT::FAILURE); 86 | return BT::FAILURE; 87 | } 88 | } 89 | } 90 | } 91 | return BT::EXIT; 92 | } 93 | 94 | int BT::FallbackNode::DrawType() 95 | { 96 | // Lock acquistion 97 | 98 | return BT::SELECTOR; 99 | } 100 | -------------------------------------------------------------------------------- /src/fallback_node_with_memory.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::FallbackNodeWithMemory::FallbackNodeWithMemory(std::string name) : ControlNode::ControlNode(name) 17 | { 18 | reset_policy_ = BT::ON_SUCCESS_OR_FAILURE; 19 | current_child_idx_ = 0; // initialize the current running child 20 | } 21 | 22 | 23 | BT::FallbackNodeWithMemory::FallbackNodeWithMemory(std::string name, int reset_policy) : ControlNode::ControlNode(name) 24 | { 25 | reset_policy_ = reset_policy; 26 | current_child_idx_ = 0; // initialize the current running child 27 | } 28 | 29 | 30 | BT::FallbackNodeWithMemory::~FallbackNodeWithMemory() {} 31 | 32 | 33 | BT::ReturnStatus BT::FallbackNodeWithMemory::Tick() 34 | { 35 | DEBUG_STDOUT(get_name() << " ticked, memory counter: "<< current_child_idx_); 36 | 37 | // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 38 | N_of_children_ = children_nodes_.size(); 39 | 40 | // Routing the ticks according to the fallback node's (with memory) logic: 41 | while (current_child_idx_ < N_of_children_) 42 | { 43 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 44 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 45 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 46 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 47 | */ 48 | 49 | if (children_nodes_[current_child_idx_]->get_type() == BT::ACTION_NODE) 50 | { 51 | // 1) If the child i is an action, read its state. 52 | // Action nodes runs in another thread, hence you cannot retrieve the status just by executing it. 53 | 54 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 55 | DEBUG_STDOUT(get_name() << " It is an action " << children_nodes_[current_child_idx_]->get_name() 56 | << " with status: " << child_i_status_); 57 | 58 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 59 | { 60 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 61 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[current_child_idx_]->get_name()); 62 | children_nodes_[current_child_idx_]->tick_engine.Tick(); 63 | 64 | // waits for the tick to arrive to the child 65 | do 66 | { 67 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 68 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 69 | } 70 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 71 | && child_i_status_ != BT::FAILURE); 72 | } 73 | } 74 | else 75 | { 76 | // 2) if it's not an action: 77 | // Send the tick and wait for the response; 78 | child_i_status_ = children_nodes_[current_child_idx_]->Tick(); 79 | children_nodes_[current_child_idx_]->set_status(child_i_status_); 80 | 81 | } 82 | 83 | 84 | if (child_i_status_ == BT::SUCCESS ||child_i_status_ == BT::FAILURE ) 85 | { 86 | // the child goes in idle if it has returned success or failure. 87 | children_nodes_[current_child_idx_]->set_status(BT::IDLE); 88 | } 89 | 90 | if (child_i_status_ != BT::FAILURE) 91 | { 92 | // If the child status is not success, return the status 93 | DEBUG_STDOUT("the status of: " << get_name() << " becomes " << child_i_status_); 94 | if (child_i_status_ == BT::SUCCESS && (reset_policy_ == BT::ON_SUCCESS 95 | || reset_policy_ == BT::ON_SUCCESS_OR_FAILURE)) 96 | { 97 | current_child_idx_ = 0; 98 | } 99 | set_status(child_i_status_); 100 | return child_i_status_; 101 | } 102 | else if (current_child_idx_ != N_of_children_ - 1) 103 | { 104 | // If the child status is failure, continue to the next child 105 | // (if any, hence if(current_child_ != N_of_children_ - 1) ) in the for loop (if any). 106 | current_child_idx_++; 107 | } 108 | else 109 | { 110 | // If it the last child. 111 | if (child_i_status_ == BT::FAILURE) 112 | { 113 | // if it the last child and it has returned failure, reset the memory 114 | current_child_idx_ = 0; 115 | } 116 | set_status(child_i_status_); 117 | return child_i_status_; 118 | } 119 | } 120 | return BT::EXIT; 121 | } 122 | 123 | 124 | int BT::FallbackNodeWithMemory::DrawType() 125 | { 126 | return BT::SELECTORSTAR; 127 | } 128 | 129 | void BT::FallbackNodeWithMemory::Halt() 130 | { 131 | current_child_idx_ = 0; 132 | BT::ControlNode::Halt(); 133 | } 134 | 135 | 136 | -------------------------------------------------------------------------------- /src/leaf_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * 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: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * 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, 11 | * 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. 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | BT::LeafNode::LeafNode(std::string name) : TreeNode(name) {} 18 | 19 | BT::LeafNode::~LeafNode() {} 20 | 21 | 22 | void BT::LeafNode::ResetColorState() 23 | { 24 | color_status_ = BT::IDLE; 25 | } 26 | 27 | int BT::LeafNode::Depth() 28 | { 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /src/parallel_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * 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: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * 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, 11 | * 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. 12 | */ 13 | 14 | 15 | #include 16 | #include 17 | 18 | BT::ParallelNode::ParallelNode(std::string name, int threshold_M) : ControlNode::ControlNode(name) 19 | { 20 | threshold_M_ = threshold_M; 21 | } 22 | 23 | BT::ParallelNode::~ParallelNode() {} 24 | 25 | BT::ReturnStatus BT::ParallelNode::Tick() 26 | { 27 | success_childred_num_ = 0; 28 | failure_childred_num_ = 0; 29 | // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 30 | N_of_children_ = children_nodes_.size(); 31 | 32 | // Routing the tree according to the sequence node's logic: 33 | for (unsigned int i = 0; i < N_of_children_; i++) 34 | { 35 | DEBUG_STDOUT(get_name() << "TICKING " << children_nodes_[i]->get_name()); 36 | 37 | if (children_nodes_[i]->get_type() == BT::ACTION_NODE) 38 | { 39 | // 1) If the child i is an action, read its state. 40 | // Action nodes runs in another parallel, hence you cannot retrieve the status just by executing it. 41 | child_i_status_ = children_nodes_[i]->get_status(); 42 | 43 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 44 | { 45 | // 1.1 If the action status is not running, the sequence node sends a tick to it. 46 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[i]->get_name()); 47 | children_nodes_[i]->tick_engine.Tick(); 48 | 49 | // waits for the tick to arrive to the child 50 | do 51 | { 52 | child_i_status_ = children_nodes_[i]->get_status(); 53 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 54 | } 55 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 56 | && child_i_status_ != BT::FAILURE); 57 | } 58 | } 59 | else 60 | { 61 | child_i_status_ = children_nodes_[i]->Tick(); 62 | } 63 | switch (child_i_status_) 64 | { 65 | case BT::SUCCESS: 66 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned success. 67 | if (++success_childred_num_ == threshold_M_) 68 | { 69 | success_childred_num_ = 0; 70 | failure_childred_num_ = 0; 71 | HaltChildren(0); // halts all running children. The execution is done. 72 | set_status(child_i_status_); 73 | return child_i_status_; 74 | } 75 | break; 76 | case BT::FAILURE: 77 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned failure. 78 | if (++failure_childred_num_ > N_of_children_- threshold_M_) 79 | { 80 | DEBUG_STDOUT("*******PARALLEL" << get_name() 81 | << " FAILED****** failure_childred_num_:" << failure_childred_num_); 82 | 83 | success_childred_num_ = 0; 84 | failure_childred_num_ = 0; 85 | HaltChildren(0); // halts all running children. The execution is hopeless. 86 | set_status(child_i_status_); 87 | return child_i_status_; 88 | } 89 | break; 90 | case BT::RUNNING: 91 | set_status(child_i_status_); 92 | break; 93 | default: 94 | break; 95 | } 96 | } 97 | return BT::RUNNING; 98 | } 99 | 100 | void BT::ParallelNode::Halt() 101 | { 102 | success_childred_num_ = 0; 103 | failure_childred_num_ = 0; 104 | BT::ControlNode::Halt(); 105 | } 106 | 107 | int BT::ParallelNode::DrawType() 108 | { 109 | return BT::PARALLEL; 110 | } 111 | 112 | 113 | unsigned int BT::ParallelNode::get_threshold_M() 114 | { 115 | return threshold_M_; 116 | } 117 | 118 | void BT::ParallelNode::set_threshold_M(unsigned int threshold_M) 119 | { 120 | threshold_M_ = threshold_M; 121 | } 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /src/sequence_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * 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: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * 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, 11 | * 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. 12 | */ 13 | 14 | 15 | #include 16 | #include 17 | 18 | 19 | BT::SequenceNode::SequenceNode(std::string name) : ControlNode::ControlNode(name) {} 20 | 21 | BT::SequenceNode::~SequenceNode() {} 22 | 23 | BT::ReturnStatus BT::SequenceNode::Tick() 24 | { 25 | // gets the number of children. The number could change if, at runtime, one edits the tree. 26 | N_of_children_ = children_nodes_.size(); 27 | 28 | // Routing the ticks according to the sequence node's logic: 29 | 30 | for (unsigned int i = 0; i < N_of_children_; i++) 31 | { 32 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 33 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 34 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 35 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 36 | */ 37 | if (children_nodes_[i]->get_type() == BT::ACTION_NODE) 38 | { 39 | // 1) If the child i is an action, read its state. 40 | child_i_status_ = children_nodes_[i]->get_status(); 41 | 42 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 43 | { 44 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 45 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[i]->get_name()); 46 | children_nodes_[i]->tick_engine.Tick(); 47 | 48 | // waits for the tick to arrive to the child 49 | do 50 | { 51 | child_i_status_ = children_nodes_[i]->get_status(); 52 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 53 | } 54 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 55 | && child_i_status_ != BT::FAILURE); 56 | } 57 | } 58 | else 59 | { 60 | // 2) if it's not an action: 61 | // Send the tick and wait for the response; 62 | child_i_status_ = children_nodes_[i]->Tick(); 63 | children_nodes_[i]->set_status(child_i_status_); 64 | } 65 | // Ponderate on which status to send to the parent 66 | if (child_i_status_ != BT::SUCCESS) 67 | { 68 | // If the child status is not success, halt the next children and return the status to your parent. 69 | if (child_i_status_ == BT::FAILURE) 70 | { 71 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned failure. 72 | } 73 | 74 | DEBUG_STDOUT(get_name() << " is HALTING children from " << (i+1)); 75 | HaltChildren(i+1); 76 | set_status(child_i_status_); 77 | return child_i_status_; 78 | } 79 | else 80 | { 81 | // the child returned success. 82 | children_nodes_[i]->set_status(BT::IDLE); 83 | 84 | if (i == N_of_children_ - 1) 85 | { 86 | // If the child status is success, and it is the last child to be ticked, 87 | // then the sequence has succeeded. 88 | set_status(BT::SUCCESS); 89 | return BT::SUCCESS; 90 | } 91 | } 92 | } 93 | return BT::EXIT; 94 | } 95 | 96 | int BT::SequenceNode::DrawType() 97 | { 98 | return BT::SEQUENCE; 99 | } 100 | -------------------------------------------------------------------------------- /src/sequence_node_with_memory.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * 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: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * 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, 11 | * 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. 12 | */ 13 | 14 | 15 | #include 16 | #include 17 | 18 | 19 | BT::SequenceNodeWithMemory::SequenceNodeWithMemory(std::string name) : ControlNode::ControlNode(name) 20 | { 21 | reset_policy_ = BT::ON_SUCCESS_OR_FAILURE; 22 | current_child_idx_ = 0; // initialize the current running child 23 | } 24 | 25 | 26 | BT::SequenceNodeWithMemory::SequenceNodeWithMemory(std::string name, int reset_policy) : ControlNode::ControlNode(name) 27 | { 28 | reset_policy_ = reset_policy; 29 | current_child_idx_ = 0; // initialize the current running child 30 | } 31 | 32 | 33 | BT::SequenceNodeWithMemory::~SequenceNodeWithMemory() {} 34 | 35 | 36 | BT::ReturnStatus BT::SequenceNodeWithMemory::Tick() 37 | { 38 | DEBUG_STDOUT(get_name() << " ticked, memory counter: "<< current_child_idx_); 39 | 40 | // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 41 | N_of_children_ = children_nodes_.size(); 42 | 43 | // Routing the ticks according to the sequence node's (with memory) logic: 44 | while (current_child_idx_ < N_of_children_) 45 | { 46 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 47 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 48 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 49 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 50 | */ 51 | 52 | if (children_nodes_[current_child_idx_]->get_type() == BT::ACTION_NODE) 53 | { 54 | // 1) If the child i is an action, read its state. 55 | // Action nodes runs in another thread, hence you cannot retrieve the status just by executing it. 56 | 57 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 58 | DEBUG_STDOUT(get_name() << " It is an action " << children_nodes_[current_child_idx_]->get_name() 59 | << " with status: " << child_i_status_); 60 | 61 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 62 | { 63 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 64 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[current_child_idx_]->get_name()); 65 | children_nodes_[current_child_idx_]->tick_engine.Tick(); 66 | 67 | // waits for the tick to arrive to the child 68 | do 69 | { 70 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 71 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 72 | } 73 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 74 | && child_i_status_ != BT::FAILURE); 75 | } 76 | } 77 | else 78 | { 79 | // 2) if it's not an action: 80 | // Send the tick and wait for the response; 81 | child_i_status_ = children_nodes_[current_child_idx_]->Tick(); 82 | children_nodes_[current_child_idx_]->set_status(child_i_status_); 83 | 84 | } 85 | 86 | if (child_i_status_ == BT::SUCCESS ||child_i_status_ == BT::FAILURE ) 87 | { 88 | // the child goes in idle if it has returned success or failure. 89 | children_nodes_[current_child_idx_]->set_status(BT::IDLE); 90 | } 91 | 92 | if (child_i_status_ != BT::SUCCESS) 93 | { 94 | // If the child status is not success, return the status 95 | DEBUG_STDOUT("the status of: " << get_name() << " becomes " << child_i_status_); 96 | if (child_i_status_ == BT::FAILURE && (reset_policy_ == BT::ON_FAILURE 97 | || reset_policy_ == BT::ON_SUCCESS_OR_FAILURE)) 98 | { 99 | current_child_idx_ = 0; 100 | } 101 | set_status(child_i_status_); 102 | return child_i_status_; 103 | } 104 | else if (current_child_idx_ != N_of_children_ - 1) 105 | { 106 | // If the child status is success, continue to the next child 107 | // (if any, hence if(current_child_ != N_of_children_ - 1) ) in the for loop (if any). 108 | current_child_idx_++; 109 | } 110 | else 111 | { 112 | // if it the last child. 113 | if (child_i_status_ == BT::SUCCESS) 114 | { 115 | // if it the last child and it has returned SUCCESS, reset the memory 116 | current_child_idx_ = 0; 117 | } 118 | set_status(child_i_status_); 119 | return child_i_status_; 120 | } 121 | } 122 | return BT::EXIT; 123 | } 124 | 125 | 126 | int BT::SequenceNodeWithMemory::DrawType() 127 | { 128 | return BT::SEQUENCESTAR; 129 | } 130 | 131 | 132 | void BT::SequenceNodeWithMemory::Halt() 133 | { 134 | current_child_idx_ = 0; 135 | BT::ControlNode::Halt(); 136 | } 137 | -------------------------------------------------------------------------------- /src/tick_engine.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | 15 | TickEngine::TickEngine(int initial_value) 16 | { 17 | value_ = initial_value; 18 | } 19 | 20 | TickEngine::~TickEngine() {} 21 | 22 | void TickEngine::Wait() 23 | { 24 | // Lock acquire (need a unique lock for the condition variable usage) 25 | std::unique_lock UniqueLock(mutex_); 26 | 27 | // If the state is 0 then we have to wait for a signal 28 | if (value_ == 0) 29 | condition_variable_.wait(UniqueLock); 30 | 31 | // Once here we decrement the state 32 | value_--; 33 | } 34 | 35 | void TickEngine::Tick() 36 | { 37 | // Lock acquire 38 | 39 | std::lock_guard LockGuard(mutex_); 40 | 41 | // State increment 42 | value_++; 43 | 44 | // Notification 45 | condition_variable_.notify_all(); 46 | } 47 | -------------------------------------------------------------------------------- /src/tree.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | 14 | #include 15 | 16 | 17 | int main(int argc, char **argv) 18 | { 19 | ros::init(argc, argv, "BehaviorTree"); 20 | try 21 | { 22 | int TickPeriod_milliseconds = 1000; 23 | 24 | BT::ActionTestNode* action1 = new BT::ActionTestNode("Action 1"); 25 | // BT::ConditionTestNode* condition1 = new BT::ConditionTestNode("Condition 1"); // commented-out as unused 26 | BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1"); 27 | 28 | 29 | BT::ActionTestNode* action2 = new BT::ActionTestNode("Action 2"); 30 | BT::ConditionTestNode* condition2 = new BT::ConditionTestNode("Condition 2"); 31 | BT:: SequenceNode* sequence2 = new BT::SequenceNode("seq1"); 32 | 33 | BT::ActionTestNode* action3 = new BT::ActionTestNode("Action 3"); 34 | BT::ConditionTestNode* condition3 = new BT::ConditionTestNode("Condition 3"); 35 | BT:: SequenceNode* sequence3 = new BT::SequenceNode("seq1"); 36 | 37 | 38 | // Commented-out as unused variables 39 | // BT::ActionTestNode* action4 = new BT::ActionTestNode("Action 4"); 40 | // BT::ConditionTestNode* condition4 = new BT::ConditionTestNode("Condition 4"); 41 | // BT:: SequenceNode* sequence4 = new BT::SequenceNode("seq1"); 42 | 43 | 44 | sequence1->AddChild(condition2); 45 | sequence1->AddChild(action1); 46 | sequence1->AddChild(sequence2); 47 | sequence1->AddChild(action3); 48 | sequence1->AddChild(sequence2); 49 | 50 | sequence2->AddChild(action2); 51 | sequence2->AddChild(condition2); 52 | 53 | sequence3->AddChild(condition3); 54 | sequence3->AddChild(action3); 55 | 56 | Execute(sequence1, TickPeriod_milliseconds); // from BehaviorTree.cpp 57 | } 58 | catch (BT::BehaviorTreeException& Exception) 59 | { 60 | std::cout << Exception.what() << std::endl; 61 | } 62 | 63 | return 0; 64 | } 65 | 66 | 67 | -------------------------------------------------------------------------------- /src/tree_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * 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: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * 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, 10 | * 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. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | 17 | BT::TreeNode::TreeNode(std::string name) : tick_engine(0) 18 | { 19 | // Initialization 20 | name_ = name; 21 | is_state_updated_ = false; 22 | set_status(BT::IDLE); 23 | } 24 | 25 | BT::TreeNode::~TreeNode() {} 26 | 27 | void BT::TreeNode::set_status(ReturnStatus new_status) 28 | { 29 | if (new_status != BT::IDLE) 30 | { 31 | set_color_status(new_status); 32 | } 33 | 34 | // Lock acquistion 35 | std::unique_lock UniqueLock(state_mutex_); 36 | 37 | // state_ update 38 | status_ = new_status; 39 | } 40 | 41 | BT::ReturnStatus BT::TreeNode::get_status() 42 | { 43 | // Lock acquistion 44 | DEBUG_STDOUT(get_name() << " is setting its status to " << status_); 45 | 46 | std::lock_guard LockGuard(state_mutex_); 47 | 48 | return status_; 49 | } 50 | 51 | BT::ReturnStatus BT::TreeNode::get_color_status() 52 | { 53 | // Lock acquistion 54 | std::lock_guard LockGuard(color_state_mutex_); 55 | 56 | return color_status_; 57 | } 58 | 59 | void BT::TreeNode::set_color_status(ReturnStatus new_color_status) 60 | { 61 | // Lock acquistion 62 | std::lock_guard LockGuard(color_state_mutex_); 63 | // state_ update 64 | color_status_ = new_color_status; 65 | } 66 | 67 | float BT::TreeNode::get_x_pose() 68 | { 69 | return x_pose_; 70 | } 71 | 72 | void BT::TreeNode::set_x_pose(float x_pose) 73 | { 74 | x_pose_ = x_pose; 75 | } 76 | 77 | 78 | float BT::TreeNode::get_x_shift() 79 | { 80 | return x_shift_; 81 | } 82 | 83 | void BT::TreeNode::set_x_shift(float x_shift) 84 | { 85 | x_shift_ = x_shift; 86 | } 87 | 88 | void BT::TreeNode::set_name(std::string new_name) 89 | { 90 | name_ = new_name; 91 | } 92 | 93 | std::string BT::TreeNode::get_name() 94 | { 95 | return name_; 96 | } 97 | 98 | BT::NodeType BT::TreeNode::get_type() 99 | { 100 | return type_; 101 | } 102 | 103 | bool BT::TreeNode::is_halted() 104 | { 105 | return get_status() == BT::HALTED; 106 | } 107 | -------------------------------------------------------------------------------- /templates/action_node_template.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | BT::CLASSNAME::CONSTRUCTOR(std::string name) : ActionNode::ActionNode(name) 5 | { 6 | thread_ = std::thread(&ActionTestNode::WaitForTick, this); 7 | } 8 | 9 | BT::CLASSNAME::~CONSTRUCTOR() {} 10 | 11 | void BT::CLASSNAME::WaitForTick() 12 | { 13 | while(true) 14 | { 15 | 16 | // Waiting for the first tick to come 17 | DEBUG_STDOUT(get_name() << " WAIT FOR TICK"); 18 | 19 | tick_engine.Wait(); 20 | DEBUG_STDOUT(get_name() << " TICK RECEIVED"); 21 | 22 | // Running state 23 | set_status(BT::RUNNING); 24 | // Perform action... 25 | 26 | while(get_status() != BT::HALTED) 27 | { 28 | /*HERE THE CODE TO EXECUTE FOR THE ACTION. 29 | wHEN THE ACTION HAS FINISHED CORRECLTY, CALL set_status(BT::SUCCESS) 30 | IF THE ACTION FAILS, CALL set_status(BT::FAILURE)*/ 31 | 32 | } 33 | } 34 | } 35 | 36 | void BT::CLASSNAME::Halt() 37 | { 38 | /*HERE THE CODE TO PERFORM WHEN THE ACTION IS HALTED*/ 39 | set_status(BT::HALTED); 40 | DEBUG_STDOUT("HALTED state set!"); 41 | } 42 | 43 | 44 | -------------------------------------------------------------------------------- /templates/condition_node_template.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | BT::CLASSNAME::CONSTRUCTOR(std::string name) : ConditionNode::ConditionNode(name) 5 | { 6 | 7 | } 8 | 9 | BT::CLASSNAME::~CONSTRUCTOR() {} 10 | 11 | BT::ReturnStatus BT::CLASSNAME::Tick() 12 | { 13 | // Condition checking and state update 14 | 15 | if (/*CONDITION TO CHECK*/) 16 | { 17 | set_status(BT::SUCCESS); 18 | return BT::SUCCESS; 19 | } 20 | else 21 | { 22 | set_status(BT::FAILURE); 23 | return BT::FAILURE; 24 | } 25 | } 26 | 27 | --------------------------------------------------------------------------------