├── .gitignore ├── CMakeLists.txt ├── Jenkinsfile ├── LICENSE ├── README.md ├── example └── example.py ├── package.xml ├── setup.py ├── src └── task_behavior_engine │ ├── __init__.py │ ├── branch.py │ ├── decorator.py │ ├── node.py │ └── tree.py └── test ├── test_branch.py ├── test_decorator.py └── test_tree.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(task_behavior_engine) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package( 9 | ) 10 | 11 | ########### 12 | ## Build ## 13 | ########### 14 | 15 | ############# 16 | ## Install ## 17 | ############# 18 | install(DIRECTORY example 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 20 | ) 21 | 22 | 23 | ############# 24 | ## Testing ## 25 | ############# 26 | 27 | ## Add folders to be run by python nosetests 28 | catkin_add_nosetests(test) 29 | -------------------------------------------------------------------------------- /Jenkinsfile: -------------------------------------------------------------------------------- 1 | // define a tag according to the Docker tag rules https://docs.docker.com/engine/reference/commandline/tag/ 2 | // the hash sign (#) is problematic when using it in bash, instead of working around this problem, just replace all 3 | // punctuation with dash (-) 4 | def projectShortName = "${env.JOB_NAME}" - "/${env.BRANCH_NAME}" 5 | def githubOrg = "ToyotaResearchInstitute" 6 | def dockerTag = "${env.BRANCH_NAME}-${env.BUILD_NUMBER}".toLowerCase().replaceAll("\\p{Punct}", "-").replaceAll("\\p{Space}", "-") 7 | 8 | def buildLink = "<${env.BUILD_URL}|${env.JOB_NAME} ${env.BUILD_NUMBER}>" 9 | 10 | node { 11 | timestamps { 12 | ansiColor('xterm') { 13 | try { 14 | properties properties: [ 15 | [$class: 'BuildDiscarderProperty', strategy: [$class: 'LogRotator', artifactDaysToKeepStr: '', artifactNumToKeepStr: '', daysToKeepStr: '30', numToKeepStr: '50']], 16 | [$class: 'GithubProjectProperty', displayName: '', projectUrlStr: "https://github.com/$githubOrg/$projectShortName"], 17 | pipelineTriggers([pollSCM('H/2 * * * *')]) 18 | ] 19 | 20 | slackSend color: 'warning', message: "build $buildLink started" 21 | stage('checkout') { 22 | withEnv(["PATH+WSTOOL=${tool 'wstool'}/bin"]) { 23 | sh """ 24 | rm -fr catkin_ws 25 | """ 26 | dir('catkin_ws/src'){ 27 | sh """ 28 | wstool init . 29 | """ 30 | dir('task_behavior_engine'){ 31 | checkout scm 32 | } 33 | } 34 | } 35 | } 36 | stage('build') { 37 | withEnv(["PATH+CATKIN=${tool 'catkin'}/bin"]) { 38 | sh """ 39 | . /opt/ros/indigo/setup.sh 40 | catkin_make install -C catkin_ws 41 | """ 42 | 43 | slackSend color: 'good', message: "stage 'build' of build $buildLink passed" 44 | } 45 | } 46 | stage('test') { 47 | withEnv(["PATH+CATKIN=${tool 'catkin'}/bin"]) { 48 | sh """ 49 | . catkin_ws/install/setup.sh 50 | catkin_make run_tests -C catkin_ws 51 | catkin_test_results 52 | """ 53 | slackSend color: '#0000FF', message: "stage 'test' of build $buildLink passed" 54 | } 55 | } 56 | } catch(Exception e) { 57 | slackSend color: 'danger', message: "build $buildLink failed" 58 | error "error building, ${e.getMessage()}" 59 | } 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Behavior Tree Engine # 2 | This package is a standalone python based behavior tree engine for robotics. If you'd like ROS integration, check out the [task_behavior_ros](https://github.com/ToyotaResearchInstitute/task_behavior_ros) package. 3 | 4 | There are several good webpages describing behavior trees in general: 5 | 6 | [Behavior trees for AI: How they work](http://www.gamasutra.com/blogs/ChrisSimpson/20140717/221339/Behavior_trees_for_AI_How_they_work.php) 7 | 8 | [Behavior tree Wikipedia](https://en.wikipedia.org/wiki/Behavior_tree_(artificial_intelligence,_robotics_and_control)) 9 | 10 | 11 | ## Background ## 12 | Behavior trees (BTs) have been well proven in the game developer community, being used in high profile video games such as Halo, Bioshock, and Spore (to name a few). Since behavior trees are, by definition, non self-referential in that leaf states should never point back to parent states, it makes it very easiy to decompose complex tasks into simpler ones without having to be concerned about how subtasks will fit into a larger task as a whole. Additionally, BTs are fairly easy to use and understand, which makes them less error prone and easier to debug. 13 | 14 | ## Key concepts ## 15 | There are several key concepts of a behavior tree. Behavior trees can be composed of three types of nodes: a control flow node (called *'behavior'* here), a state modifying node (called a *'decorator'* here) and an execution node (called a *'node'* here) which does the actual work. The entire tree gets ticked, or run, on each update cycle and as such it is important that the execution nodes are non-blocking, though this isn't strictly enforced. Each execution node can return 1 of 3 states: active, success, or fail (though I have added some additional states for book-keeping and execution reasons). The limited number of return states is a part of the magic that makes BTs modularity work. 16 | 17 | ### Behaviors ### 18 | These are the control flow nodes. By definition they are nodes who have children and control how and when their children are executed. In this implementation, these nodes are free to have as many children as desired. Several different types of control flows are included in this package (in branch.py) and should all be well documented within that file. I will, however explain some key points using examples here. 19 | 20 | The two most common behavior tree control nodes are 'selector' and 'sequencer'. A selector runs each child, in order, until one succeeds. If the child being executed returns "active", that child will continue to be ticked (assessed/run) until a terminal state is reached. The 'selector' acts as a type of fail-over which allows for recovery behaviors. 21 | 22 | **example)** *Say you want your robot to move forward, but for some reason that direction is blocked, you could try the next-best motion 'move right', and do that until forward motion is possible.* 23 | 24 | It is important to note that behaviors are re-evaluated at each tick and that this particular one will from the first child at every tick, so we won't get "stuck" in move right when forward motion is desired. 25 | 26 | A sequencer runs each child, in order, until one fails. If the child being executed returns "active", that child will continue to be ticked until a terminal state is reached. The 'sequencer' acts as a type of guarded queue in which the tree can assert certain conditions are met before continuing. 27 | 28 | **example)** *Say you want your robot to roam around the room, but you want to make sure you have enough battery power to make it back to base, you can first check that battery level is ok before sending the next waypoint.* 29 | 30 | A failed child will stop the behavior of the current task and report a failure up the tree so that appropriate actions can take place. 31 | 32 | As you can imagine, there are many different control-flow schemas that can be created here, including those that are machine-learned (which I am very excited about trying!). 33 | 34 | ### Decorators ### 35 | These are state-modifying nodes. By definition they have a singular child and their purpose is to change the reported output state of the execution node so that they are executed in the higher level task as desired (without having to change the actual execution node). Several different decorators have been defined in this package (in decorator.py) and should all be well documented within that file. A common example is the "repeat" and "negate" decorators. 36 | 37 | The "repeat" decorator will report "active" regardless of its child state at every tick. In essense this will ensure that the child is always run as it will never report a terminal state to the parent behavior. 38 | 39 | The "negate" will switch the output state of its child node from "success" to "fail" or vice-versa. This effectifly adds a "not" to the output and is very useful for reversing limit checks (is it a success to be under or over this value?) without having to re-create the actual check. 40 | 41 | ### Nodes ### 42 | These are the things that do the actual work. In this implementation, it is best that the work be 'light' and non-blocking as it will allow the tree to react to other inputs. Nodes should be designed to do one thing, and to do it well. Obviously in robotics there are many long-running/blocking actions that will need to happen, such as planning or perception or even trajectory execution. These things should still happen, but *outside* the behavior tree evaluation loop. These should be run in separate threads or processes and the current status of them should be reported back to the tree. Note that this isn't strictly enforced and you could have blocking nodes if you desire, but it is not encouraged. 43 | 44 | ### Data ### 45 | Each node has access to a data structure which allows the nodes to exchange information and affect each other's execution. The current implementation seperates the data available for the node *'NodeData'* so that each node only has access to its own data while allowing for remapping from one node's data to another via a *'Blackboard'*. At the moment blackboards can be shared at any level of the tree if needed, but need to be cascaded down. I'm not completely sold on my implementation of this structure, and encourage a healthy debate on the matter. 46 | 47 | ## Organization ## 48 | This package is intended to be a stand-alone implementation of a python-based behavior tree engine which has limited outside dependencies (specifically, no ROS dependency). Please see [task_behavior_ros](https://github.com/ToyotaResearchInstitute/task_behavior_ros) for ROS support. 49 | 50 | * **tree.py** This holds the core implementation of the behavior tree. The base structure of a 'behavior', 'decorator' and 'node' is defined here. Note that 'behavior' and 'decorators' are inherited from a 'node' and therefore possess all of a nodes properties. 51 | * **branch.py** This holds the main control-flow behaviors -- There are many! 52 | * **decorator.py** This holds the main output-modifying decorators -- There are many! 53 | * **node.py** While I, in general, would discourage the inclusion of execution nodes in this package as they should be very specific to the application, I have added a few that I found useful for testing as well as execution (think no-ops). 54 | 55 | * **test/** In an effort to make the core stable and awesome I have added somewhat extensive unit tests which can be found [here](https://github.com/ToyotaResearchInstitute/task_behavior_engine/tree/master/test). If something is not working as expected, I highly encourage you to file an issue, or a PR with a test that recreates the problem. 56 | 57 | ## Getting Started ## 58 | 59 | ### Installation ### 60 | ROS Integration is provided by the [task_behavior_ros](https://github.com/ToyotaResearchInstitute/task_behavior_ros) package. If you are intending on using this package with ROS, ignore these install directions and follow the ones in [task_behavior_ros](https://github.com/ToyotaResearchInstitute/task_behavior_ros). 61 | 62 | ```bash 63 | # Install required libraries 64 | sudo apt-get update 65 | sudo apt-get install -y \ 66 | cmake \ 67 | git \ 68 | python-pip 69 | sudo pip install catkin_pkg 70 | ``` 71 | ```bash 72 | # Install the package 73 | git clone https://github.com/ToyotaResearchInstitute/task_behavior_engine.git 74 | cd task_behavior_engine 75 | sudo python setup.py install 76 | ``` 77 | 78 | ## Usage ## 79 | It's pretty easy to use this library to create a behavior tree (it's python!) The minimum amount of work that needs to be done is 80 | 81 | 1. Create a node 82 | 2. Create the update loop 83 | 84 | The below example can be found in [example/example.py](https://github.com/ToyotaResearchInstitute/task_behavior_engine/blob/master/example/example.py). 85 | ### Create a node ### 86 | First, you will want to either create or use an execution node: 87 | 88 | ``` python 89 | class Count(Node): 90 | def __init__(self, name, *args, **kwargs): 91 | super(Count, self).__init__(name, 92 | run_cb = self.run, 93 | configure_cb = self.configure, 94 | cleanup_cb = self.cleanup, 95 | *args, **kwargs) 96 | 97 | def configure(self, nodedata): 98 | self.start = nodedata.get_data('start', 0) 99 | self.limit = nodedata.get_data('limit', 5) 100 | nodedata.index = self.start 101 | 102 | def run(self, nodedata): 103 | if nodedata.index < self.limit: 104 | nodedata.index += 1 105 | return NodeStatus(NodeStatus.ACTIVE, "Count " + str(nodedata.index)) 106 | return NodeStatus(NodeStatus.SUCCESS, "Count finished at " + str(nodedata.index)) 107 | 108 | def cleanup(self, nodedata): 109 | nodedata.index = self.start 110 | ``` 111 | 112 | Each behavior tree node inherits from the Node class. You will want to initialize it with a (human) readable name to aid in debugging. Next, you will want to initialize the parent class. Inside of the parent class initialization you can setup your callbacks. Available callbacks are: 113 | 114 | * **run\_cb**: _required_ This callback is called at every tick when the node is active. 115 | * **configure\_cb**: This callback is called before the first execution of the node (before run\_cb). In general it will be used to set up variables before execution. 116 | * **cleanup\_cb**: This callback is called after execution has terminated, meaning the node has reported a SUCCESS, FAIL, or CANCEL condition. 117 | * **cancel\_cb**: This callback is called when a CANCEL condition is triggered. 118 | 119 | Now you can start filling out your callbacks to make the node execute in the way that you desire. 120 | 121 | ```python 122 | def configure(self, nodedata): 123 | self.start = nodedata.get_data('start', 0) 124 | self.limit = nodedata.get_data('limit', 5) 125 | nodedata.index = self.start 126 | ``` 127 | 128 | Here, the node is configured by looking up the start value in its data, and defaulting to 0 if it has not been set. This allows other nodes to change the initial value of the count. It also configures how high to count, defaulting to 5 if it hasn't been set by another node. 129 | 130 | ```python 131 | def run(self, nodedata): 132 | if nodedata.index < self.limit: 133 | nodedata.index += 1 134 | return NodeStatus(NodeStatus.ACTIVE, "Count " + str(nodedata.index)) 135 | return NodeStatus(NodeStatus.SUCCESS, "Count finished at " + str(nodedata.index)) 136 | ``` 137 | 138 | Next, the run_cb does its work by incrementing the count and returns a NodeStatus, which here ACTIVE while counting, and SUCCESS after the count has been reached. You may also add a string description of the status to aid in debugging (this is optional, but encouraged). 139 | 140 | ```python 141 | def cleanup(self, nodedata): 142 | nodedata.index = self.start 143 | ``` 144 | 145 | Finally, cleanup resets the index to start. 146 | 147 | 148 | ### Create the update loop ### 149 | The simplist method to update the behavior tree is to simply tick the root node. 150 | 151 | ```python 152 | if __name__ == '__main__': 153 | 154 | count = Count(name="count_index") 155 | result = NodeStatus(NodeStatus.ACTIVE) 156 | 157 | while result == NodeStatus.ACTIVE: 158 | result = count.tick() 159 | print result 160 | time.sleep(0.1) 161 | ``` 162 | 163 | ### Adding a decorator ### 164 | As explained above, decorators are designed to simply change the output of a node. So say we want count to always be active, we can just add a Repeat decorator as shown below. 165 | 166 | ```python 167 | if __name__ == '__main__': 168 | 169 | count = Count(name="count_index") 170 | repeat = Repeat(name="repeat_count", child=count) 171 | result = NodeStatus(NodeStatus.ACTIVE) 172 | 173 | for i in range(0, 10): 174 | result = repeat.tick() 175 | print result 176 | time.sleep(0.1) 177 | ``` 178 | 179 | ### Using a behavior ### 180 | A behavior has a multitude of children and determines when its children are run. If we want to sequence some counts together, we must remember that a sequencer's children are re-evaluated from the first child on every tick. This will run count1 until success and then move to count2, where it will re-start count1 and again run it until success before moving on. 181 | 182 | ```python 183 | if __name__ == '__main__': 184 | 185 | # set up common variable blackboard for all nodes 186 | b = Blackboard() 187 | 188 | count1 = Count(name="count_1", blackboard=b) 189 | count2 = Count(name="count_2", blackboard=b) 190 | count3 = Count(name="count_3", blackboard=b) 191 | 192 | finish_counts = Sequencer("finish_counts", blackboard=b) 193 | finish_counts.add_child(count1) 194 | finish_counts.add_child(count2) 195 | finish_counts.add_child(count3) 196 | 197 | # change the limit for the count 198 | b.save("limit", 1, count1._id) 199 | b.save("limit", 2, count2._id) 200 | b.save("limit", 3, count3._id) 201 | 202 | result = NodeStatus(NodeStatus.ACTIVE) 203 | 204 | while result == NodeStatus.ACTIVE: 205 | result = finish_counts.tick() 206 | print result 207 | time.sleep(0.1) 208 | ``` 209 | Here is also an example of how to use the blackboard to set node parameters. 210 | 211 | ## Demo ## 212 | The above is included in the package as an example 213 | 214 | ```bash 215 | $ ./example/example.py 216 | ``` 217 | -------------------------------------------------------------------------------- /example/example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright 2016 Toyota Research Institute 4 | 5 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 6 | # use this file except in compliance with the License. You may obtain a copy 7 | # of the License at 8 | 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 13 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 14 | # License for the specific language governing permissions and limitations 15 | # under the License. 16 | 17 | import time 18 | 19 | from task_behavior_engine.branch import Sequencer 20 | from task_behavior_engine.decorator import Repeat 21 | from task_behavior_engine.tree import Blackboard 22 | from task_behavior_engine.tree import Node 23 | from task_behavior_engine.tree import NodeStatus 24 | 25 | 26 | class Count(Node): 27 | 28 | ''' The Count node starts at some number and counts up to a limit. 29 | It will return NodeStatus.ACTIVE until the count has reached the limit, 30 | then it will return NodeStatus.SUCCESS. 31 | 32 | As a node, this inherits from the base Node class. 33 | ''' 34 | 35 | def __init__(self, name, *args, **kwargs): 36 | ''' User-facing initialization 37 | @param name [str] The human-readable name to aid in debugging 38 | ''' 39 | super(Count, self).__init__( 40 | name, 41 | run_cb=self.run, 42 | configure_cb=self.configure, 43 | cleanup_cb=self.cleanup, 44 | *args, **kwargs) 45 | 46 | def configure(self, nodedata): 47 | ''' Initialize the start and limit values 48 | @param nodedata [NodeData] The shareable data associated with this node 49 | 50 | NodeData: 51 | start [int] The number the count should start on 52 | limit [int] The number the count sound end on 53 | ''' 54 | self.start = nodedata.get_data('start', 0) 55 | self.limit = nodedata.get_data('limit', 5) 56 | nodedata.index = self.start 57 | 58 | def run(self, nodedata): 59 | ''' Update the count and return current status. 60 | @param nodedata [NodeData] The shareable data associated with this node 61 | 62 | NodeData: 63 | index [int] The current index of the count 64 | ''' 65 | if nodedata.index < self.limit: 66 | nodedata.index += 1 67 | return NodeStatus(NodeStatus.ACTIVE, "Count " + str(nodedata.index)) 68 | return NodeStatus(NodeStatus.SUCCESS, "Count finished at " + str(nodedata.index)) 69 | 70 | def cleanup(self, nodedata): 71 | ''' Reset the count to start 72 | @param nodedata [NodeData] 73 | 74 | NodeData: 75 | index [int] The current index of the count 76 | ''' 77 | nodedata.index = self.start 78 | 79 | if __name__ == '__main__': 80 | 81 | print "Running example 1 -- using a node" 82 | 83 | count = Count(name="count_index") 84 | result = NodeStatus(NodeStatus.ACTIVE) 85 | 86 | while result == NodeStatus.ACTIVE: 87 | result = count.tick() 88 | print result 89 | time.sleep(0.1) 90 | 91 | print "Running example 2 (10 times) -- using a decorator" 92 | 93 | count = Count(name="count_index") 94 | repeat = Repeat(name="repeat_count", child=count) 95 | result = NodeStatus(NodeStatus.ACTIVE) 96 | 97 | for i in range(0, 10): 98 | result = repeat.tick() 99 | print result 100 | time.sleep(0.1) 101 | 102 | print "Running example 3 -- using a behavior" 103 | 104 | b = Blackboard() 105 | 106 | count1 = Count(name="count_1", blackboard=b) 107 | count2 = Count(name="count_2", blackboard=b) 108 | count3 = Count(name="count_3", blackboard=b) 109 | 110 | finish_counts = Sequencer("finish_counts", blackboard=b) 111 | finish_counts.add_child(count1) 112 | finish_counts.add_child(count2) 113 | finish_counts.add_child(count3) 114 | 115 | # change the limit for the count 116 | b.save("limit", 1, count1._id) 117 | b.save("limit", 2, count2._id) 118 | b.save("limit", 3, count3._id) 119 | 120 | result = NodeStatus(NodeStatus.ACTIVE) 121 | 122 | while result == NodeStatus.ACTIVE: 123 | result = finish_counts.tick() 124 | print result 125 | time.sleep(0.1) 126 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | task_behavior_engine 4 | 0.0.0 5 | The task_behavior_engine package 6 | 7 | Allison Thackston 8 | 9 | Apache 2.0 10 | 11 | Allison Thackston 12 | 13 | catkin 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['task_behavior_engine',], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /src/task_behavior_engine/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ToyotaResearchInstitute/task_behavior_engine/2f7f99f53146062f1167e211e7f6c2040b2f77a2/src/task_behavior_engine/__init__.py -------------------------------------------------------------------------------- /src/task_behavior_engine/branch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | 16 | import logging 17 | import random 18 | 19 | from tree import Behavior 20 | from tree import NodeStatus 21 | 22 | logger = logging.getLogger(__name__) 23 | 24 | 25 | class Selector(Behavior): 26 | 27 | """ A Selector runs each child in order until one succeeds. 28 | This allows failover behaviors. If one succeeds, 29 | execution of this behavior stops and it returns NodeStatus.SUCCESS. 30 | If no child succeed, Selector returns NodeStatus.FAIL. 31 | If a child is still ACTIVE, then a NodeStatus.ACTIVE status is returned. 32 | """ 33 | 34 | def __init__(self, name, *args, **kwargs): 35 | super(Selector, self).__init__(name, run_cb=self.run, *args, **kwargs) 36 | 37 | def run(self, nodedata): 38 | logger.debug("Selector.run() " + str(self._children)) 39 | self.reset_children_status() 40 | for c in self._children: 41 | logger.debug("Selector.tick_child() " + c._name) 42 | result = self.tick_child(c) 43 | if result.status == NodeStatus.ACTIVE or result.status == NodeStatus.PENDING: 44 | return NodeStatus(NodeStatus.ACTIVE, 45 | str("Executing " + self._name + ":" + c._name)) 46 | if result.status == NodeStatus.SUCCESS: 47 | return NodeStatus(NodeStatus.SUCCESS, 48 | str("Successfully completed " + self._name + ":" + c._name)) 49 | 50 | return NodeStatus(NodeStatus.FAIL, str("All children failed in " + self._name)) 51 | 52 | 53 | class Sequencer(Behavior): 54 | 55 | """ A Sequencer runs through each child unless one fails. 56 | If one fails, the executiong of this behavior stops and it returns NodeStatus.FAIL. 57 | This behavior returns NodeStatus.ACTIVE until all children have finished 58 | or one failed. It returns NodeStatus.SUCCESS if all children succeed. 59 | """ 60 | 61 | def __init__(self, name, *args, **kwargs): 62 | super(Sequencer, self).__init__(name, run_cb=self.run, *args, **kwargs) 63 | 64 | def run(self, nodedata): 65 | logger.debug("Sequencer.run() " + str(self._children)) 66 | self.reset_children_status() 67 | for c in self._children: 68 | logger.debug("Sequencer.tick_child() " + c._name) 69 | result = self.tick_child(c) 70 | if result.status == NodeStatus.ACTIVE or result.status == NodeStatus.PENDING: 71 | return NodeStatus(NodeStatus.ACTIVE, 72 | str("Executing " + self._name + ":" + c._name)) 73 | if not result.status == NodeStatus.SUCCESS: 74 | return NodeStatus(NodeStatus.FAIL, 75 | str("Failed to complete " + self._name + ":" + c._name)) 76 | 77 | return NodeStatus(NodeStatus.SUCCESS, str("All children succeeded in " + self._name)) 78 | 79 | 80 | class Runner(Behavior): 81 | 82 | """ A Runner runs through all children in order regardless of success/fail. 83 | It returns NodeStatus.ACTIVE while children are running. 84 | It always returns Status.SUCCESS after all children have completed. 85 | """ 86 | 87 | def __init__(self, name, *args, **kwargs): 88 | super(Runner, self).__init__(name, run_cb=self.run, *args, **kwargs) 89 | 90 | def run(self, nodedata): 91 | logger.debug("Runner.run() " + str(self._children)) 92 | self.reset_children_status() 93 | for c in self._children: 94 | logger.debug("Runner.tick_child() " + c._name) 95 | result = self.tick_child(c) 96 | if result.status == NodeStatus.ACTIVE or result.status == NodeStatus.PENDING: 97 | return NodeStatus(NodeStatus.ACTIVE, 98 | str("Executing " + self._name + ":" + c._name)) 99 | return NodeStatus(NodeStatus.SUCCESS, str("All children finished in " + self._name)) 100 | 101 | 102 | class Any(Behavior): 103 | 104 | """ An Any runs every child at each timestep (in order). 105 | If any child succeeds, all running children are cancelled and an Any returns NodeStatus.SUCCESS. 106 | If all children fail, it returns NodeStatus.FAIL. 107 | Otherwise, an Any returns NodeStatus.ACtiVE. 108 | """ 109 | 110 | def __init__(self, name, *args, **kwargs): 111 | super(Any, self).__init__(name, 112 | configure_cb=self.configure, 113 | run_cb=self.run, 114 | *args, **kwargs) 115 | 116 | def configure(self, nodedata): 117 | logger.debug("Any.configure() " + str(self._children)) 118 | self._open_nodes = [] 119 | for child in self._children: 120 | self._open_nodes.append(child._id) 121 | 122 | def run(self, nodedata): 123 | logger.debug("Any.run() " + str(self._children)) 124 | active = False 125 | for c in self._children: 126 | if c._id in self._open_nodes: 127 | logger.debug("Any.tick_child() " + c._name) 128 | result = self.tick_child(c) 129 | if result.status == NodeStatus.SUCCESS: 130 | return NodeStatus(NodeStatus.SUCCESS, str("Found SUCCESS in " + self._name + ":" + c._name)) 131 | if result.status == NodeStatus.ACTIVE or result.status == NodeStatus.PENDING: 132 | active = True 133 | if active: 134 | return NodeStatus(NodeStatus.ACTIVE, str("Executing " + self._name)) 135 | 136 | return NodeStatus(NodeStatus.FAIL, str("Failed to complete " + self._name + ". All children failed.")) 137 | 138 | 139 | class All(Behavior): 140 | 141 | """ An All runs every child at each timestep (in order). 142 | If one fails, currently running children are canceled and an All returns NodeStatus.FAIL. 143 | If all succeed, it returns NodeStatus.SUCCESS. 144 | Otherwise, an All returns NodeStatus.ACtiVE. 145 | """ 146 | 147 | def __init__(self, name, *args, **kwargs): 148 | super(All, self).__init__(name, 149 | configure_cb=self.configure, 150 | run_cb=self.run, 151 | *args, **kwargs) 152 | 153 | def configure(self, nodedata): 154 | logger.debug("All.configure() " + str(self._children)) 155 | self._open_nodes = [] 156 | for child in self._children: 157 | self._open_nodes.append(child._id) 158 | 159 | def run(self, nodedata): 160 | logger.debug("All.run() " + str(self._children)) 161 | active = False 162 | for c in self._children: 163 | if c._id in self._open_nodes: 164 | logger.debug("All.tick_child() " + c._name) 165 | result = self.tick_child(c) 166 | if result.status == NodeStatus.FAIL: 167 | return NodeStatus(NodeStatus.FAIL, str("Found FAIL in " + self._name + ":" + c._name)) 168 | if result.status == NodeStatus.ACTIVE or result.status == NodeStatus.PENDING: 169 | active = True 170 | if active: 171 | return NodeStatus(NodeStatus.ACTIVE, str("Executing " + self._name)) 172 | 173 | return NodeStatus(NodeStatus.SUCCESS, str("All succeeded in " + self._name)) 174 | 175 | 176 | class Random(Behavior): 177 | 178 | """ A Random chooses one child at random to execute. 179 | A Random returns the result of that child. 180 | """ 181 | 182 | def __init__(self, name, *args, **kwargs): 183 | super(Random, self).__init__(name, 184 | run_cb=self.run, 185 | configure_cb=self.configure, 186 | *args, **kwargs) 187 | self.child = None 188 | 189 | def configure(self, nodedata): 190 | if len(self._children) == 0: 191 | logger.info("No children set") 192 | self.child = None 193 | return 194 | logger.debug("Random.config() " + str(self._children)) 195 | self.child = random.choice(self._children) 196 | logger.info("Selected random child: " + self.child._name) 197 | 198 | def run(self, nodedata): 199 | if self.child is None: 200 | logger.debug("Random.run() empty") 201 | return NodeStatus(NodeStatus.SUCCESS, "No child selected") 202 | logger.debug("Random.run() " + str(self.child._name)) 203 | self.reset_children_status() 204 | return self.tick_child(self.child) 205 | 206 | 207 | class Progressor(Behavior): 208 | 209 | """ A Progressor keeps track of which child is currently executing, 210 | and executes all children in order, without re-evaluating 211 | previously executed children. 212 | This behavior returns Status.ACTIVE until a child has failed 213 | or all have succeed. 214 | If a single child fails, the behavior fails. 215 | If all children succeed, the behavior succeeds. 216 | """ 217 | 218 | def __init__(self, name, *args, **kwargs): 219 | super(Progressor, self).__init__(name, 220 | configure_cb=self.configure, 221 | run_cb=self.run, 222 | *args, **kwargs) 223 | 224 | def configure(self, nodedata): 225 | logger.debug("Progressor.config()" + str(self._children)) 226 | self.index = 0 227 | 228 | def run(self, nodedata): 229 | logger.debug("Progressor.run()" + str(self._children)) 230 | for i, c in enumerate(self._children): 231 | if self.index == i: 232 | logger.debug("Progressor.tick_child() " + c._name) 233 | result = self.tick_child(c) 234 | if result.status == NodeStatus.ACTIVE or result.status == NodeStatus.PENDING: 235 | return NodeStatus(NodeStatus.ACTIVE, 236 | str("Executing " + self._name + ":" + c._name)) 237 | if not result.status == NodeStatus.SUCCESS: 238 | return NodeStatus(result.status, 239 | str("Failed to complete " + self._name + ":" + c._name)) 240 | self.index += 1 241 | 242 | return NodeStatus(NodeStatus.SUCCESS, str("All children succeeded in " + self._name)) 243 | 244 | 245 | class Majority(Behavior): 246 | 247 | """ A Majority runs every child at each timestep (in order). 248 | This behavior returns Status.ACTIVE until a majority is reached. 249 | If a majority of children fail, the behavior fails. 250 | If a majority of children succeed, the behavior succeeds. 251 | 50/50 splits err on the side of success. 252 | """ 253 | 254 | def __init__(self, name, *args, **kwargs): 255 | super(Majority, self).__init__(name, 256 | configure_cb=self.configure, 257 | run_cb=self.run, 258 | *args, **kwargs) 259 | 260 | def configure(self, nodedata): 261 | logger.debug("Majority.config() " + str(self._children)) 262 | self.num_fail = 0 263 | self.num_succeed = 0 264 | self._open_nodes = [] 265 | for child in self._children: 266 | self._open_nodes.append(child._id) 267 | 268 | def run(self, nodedata): 269 | logger.debug("Majority.run() " + str(self._children)) 270 | num_children = float(len(self._children)) 271 | for c in self._children: 272 | if c._id in self._open_nodes: 273 | logger.debug("Majority.tick_child() " + c._name) 274 | result = self.tick_child(c) 275 | if result.status == NodeStatus.FAIL: 276 | self.num_fail += 1 277 | if result.status == NodeStatus.SUCCESS: 278 | self.num_succeed += 1 279 | 280 | logger.debug("num_fail: " + str(self.num_fail)) 281 | logger.debug("num_succeed: " + str(self.num_succeed)) 282 | 283 | logger.debug( 284 | "num_fail/num_children= " + str(self.num_fail / num_children)) 285 | if self.num_fail / num_children > 0.5: 286 | return NodeStatus(NodeStatus.FAIL, "The majority of children failed") 287 | logger.debug( 288 | "num_succeed/num_children= " + str(self.num_succeed / num_children)) 289 | if self.num_succeed / num_children >= 0.5: 290 | return NodeStatus(NodeStatus.SUCCESS, "The majority of children succeeded") 291 | 292 | return NodeStatus(NodeStatus.ACTIVE, str("Executing " + self._name)) 293 | 294 | 295 | class First(Behavior): 296 | 297 | """ A First runs every child at each timestep (in order). 298 | The result of the first child to finish is returned, 299 | all other running children are canceled. 300 | """ 301 | 302 | def __init__(self, name, *args, **kwargs): 303 | super(First, self).__init__(name, run_cb=self.run, *args, **kwargs) 304 | 305 | def run(self, nodedata): 306 | logger.debug("First.run() " + str(self._children)) 307 | for c in self._children: 308 | logger.debug("First.tick_child() " + c._name) 309 | result = self.tick_child(c) 310 | if result.status == NodeStatus.FAIL or result.status == NodeStatus.SUCCESS: 311 | return result 312 | 313 | return NodeStatus(NodeStatus.ACTIVE) 314 | -------------------------------------------------------------------------------- /src/task_behavior_engine/decorator.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | import logging 16 | 17 | from tree import Decorator 18 | from tree import NodeStatus 19 | 20 | logger = logging.getLogger(__name__) 21 | 22 | 23 | class Negate(Decorator): 24 | 25 | """ A Negate decorator reverses the status of the child node. 26 | If the child returns SUCCESS, this will return FAIL. 27 | If the child returns FAIL, this will return SUCCESS. 28 | All other statuses are passed through. 29 | """ 30 | 31 | def __init__(self, name, *args, **kwargs): 32 | super(Negate, self).__init__(name=name, 33 | run_cb=self.run, 34 | *args, **kwargs) 35 | 36 | def run(self, nodedata): 37 | logger.debug("Negate.run() " + self._child._name) 38 | result = self.tick_child() 39 | if result == NodeStatus.SUCCESS: 40 | return NodeStatus(NodeStatus.FAIL, "Negating " + self._child._name) 41 | elif result == NodeStatus.FAIL: 42 | return NodeStatus(NodeStatus.SUCCESS, "Negating " + self._child._name) 43 | 44 | return result 45 | 46 | 47 | class Repeat(Decorator): 48 | 49 | """ A Repeat decorator returns ACTIVE when result is either SUCCESS or FAIL. 50 | All other statuses are passed through. 51 | """ 52 | 53 | def __init__(self, name, *args, **kwargs): 54 | super(Repeat, self).__init__(name=name, 55 | run_cb=self.run, 56 | *args, **kwargs) 57 | 58 | def run(self, nodedata): 59 | logger.debug("Repeat.run() " + self._child._name) 60 | result = self.tick_child() 61 | if result == NodeStatus.SUCCESS or result == NodeStatus.FAIL: 62 | return NodeStatus(NodeStatus.ACTIVE, "Repeating.. " + self._child._name) 63 | return result 64 | 65 | 66 | class While(Decorator): 67 | 68 | """ A While decorator returns ACTIVE while the child returns SUCCESS. 69 | All other statuses are passed through. 70 | """ 71 | 72 | def __init__(self, name, *args, **kwargs): 73 | super(While, self).__init__(name=name, 74 | run_cb=self.run, 75 | *args, **kwargs) 76 | 77 | def run(self, nodedata): 78 | logger.debug("While.run() " + self._child._name) 79 | result = self.tick_child() 80 | if result == NodeStatus.SUCCESS: 81 | return NodeStatus(NodeStatus.ACTIVE, "Continuing.. " + self._child._name) 82 | return result 83 | 84 | 85 | class Until(Decorator): 86 | 87 | """ An Until decorator returns ACTIVE while the child returns FAIL. 88 | All other statuses are passed through. 89 | """ 90 | 91 | def __init__(self, name, *args, **kwargs): 92 | super(Until, self).__init__(name=name, 93 | run_cb=self.run, 94 | *args, **kwargs) 95 | 96 | def run(self, nodedata): 97 | logger.debug("Until.run() " + self._child._name) 98 | result = self.tick_child() 99 | if result == NodeStatus.FAIL: 100 | return NodeStatus(NodeStatus.ACTIVE, "Trying again.. " + self._child._name) 101 | return result 102 | 103 | 104 | class UntilCount(Decorator): 105 | """ An UntilCount returns ACTIVE while the child returns FAIL up to count. 106 | All other statuses are passed through. 107 | """ 108 | 109 | def __init__(self, name, max_count, *args, **kwargs): 110 | super(UntilCount, self).__init__( 111 | name=name, configure_cb=self.config, run_cb=self.run, *args, **kwargs) 112 | 113 | self._max_count = max_count 114 | 115 | def config(self, nodedata): 116 | nodedata.count = 0 117 | nodedata.max_count = self._max_count 118 | 119 | def run(self, nodedata): 120 | logger.debug("UntilCount.run() " + self._child._name) 121 | result = self.tick_child() 122 | if result == NodeStatus.FAIL: 123 | nodedata.count += 1 124 | if nodedata.count < nodedata.max_count: 125 | return NodeStatus(NodeStatus.ACTIVE, "Trying again %s until %s..".format(nodedata.count, nodedata.max_count)) 126 | return result 127 | 128 | 129 | class Fail(Decorator): 130 | 131 | """ A Fail decorator returns FAIL if the child returns SUCCESS. 132 | All other statuses are passed through. 133 | 134 | A Fail will always return NodeStatus.FAIL on child completion, 135 | regardless of child SUCCESS/FAIL status. 136 | """ 137 | 138 | def __init__(self, name, *args, **kwargs): 139 | super(Fail, self).__init__(name=name, run_cb=self.run, *args, **kwargs) 140 | 141 | def run(self, nodedata): 142 | logger.debug("Fail.run() " + self._child._name) 143 | result = self.tick_child() 144 | if result == NodeStatus.SUCCESS: 145 | return NodeStatus(NodeStatus.FAIL, "Failing " + self._child._name) 146 | return result 147 | 148 | 149 | class Succeed(Decorator): 150 | 151 | """ A Succeed decorator returns SUCCESS if the child returns FAIL. 152 | All other statuses are passed through. 153 | 154 | A Success will always return NodeStatus.SUCCESS on child completion, 155 | regardless of child SUCCESS/FAIL status. 156 | """ 157 | 158 | def __init__(self, name, *args, **kwargs): 159 | super(Succeed, self).__init__(name=name, 160 | run_cb=self.run, 161 | *args, **kwargs) 162 | 163 | def run(self, nodedata): 164 | logger.debug("Succeed.run() " + self._child._name) 165 | result = self.tick_child() 166 | if result == NodeStatus.FAIL: 167 | return NodeStatus(NodeStatus.SUCCESS, "Succeeding " + self._child._name) 168 | return result 169 | -------------------------------------------------------------------------------- /src/task_behavior_engine/node.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | 16 | from task_behavior_engine.tree import Node 17 | from task_behavior_engine.tree import NodeStatus 18 | 19 | 20 | class Success(Node): 21 | 22 | """ A Success node always returns NodeStatus.SUCCESS. """ 23 | 24 | def __init__(self, name, *args, **kwargs): 25 | super(Success, self).__init__(name, run_cb=self.run, *args, **kwargs) 26 | 27 | def run(self, nodedata): 28 | return NodeStatus(NodeStatus.SUCCESS) 29 | 30 | 31 | class Fail(Node): 32 | 33 | """ A Fail node always returns NodeStatus.FAIL. """ 34 | 35 | def __init__(self, name, *args, **kwargs): 36 | super(Fail, self).__init__(name, run_cb=self.run, *args, **kwargs) 37 | 38 | def run(self, nodedata): 39 | return NodeStatus(NodeStatus.FAIL) 40 | 41 | 42 | class Continue(Node): 43 | 44 | """ A Continue node always returns NodeStatus.ACTIVE. """ 45 | 46 | def __init__(self, name, *args, **kwargs): 47 | super(Continue, self).__init__(name, run_cb=self.run, *args, **kwargs) 48 | 49 | def run(self, nodedata): 50 | return NodeStatus(NodeStatus.ACTIVE) 51 | -------------------------------------------------------------------------------- /src/task_behavior_engine/tree.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | 16 | import logging 17 | import threading 18 | import uuid 19 | 20 | logger = logging.getLogger(__name__) 21 | 22 | 23 | class NodeData(object): 24 | 25 | """ This object is a dictionary that holds all of the data for a node. 26 | You can access the members by standard dictionary calls (nodedata[name]) 27 | or via attribute (nodedata.name) 28 | """ 29 | 30 | def __init__(self): 31 | self._data = {} 32 | self._locks = {} 33 | 34 | def __contains__(self, key): 35 | return key in self._data.keys() 36 | 37 | def __getattr__(self, name): 38 | """ Override getattr to be thread safe. """ 39 | if name[0] == '_': 40 | return object.__getattr__(self, name) 41 | if not name in self._locks.keys(): 42 | self._locks[name] = threading.Lock() 43 | 44 | with self._locks[name]: 45 | temp = self._data[name] 46 | 47 | return temp 48 | 49 | def __setattr__(self, name, value): 50 | """ Override setattr to be thread safe. """ 51 | if name[0] == '_': 52 | return object.__setattr__(self, name, value) 53 | 54 | if not name in self._locks.keys(): 55 | self._locks[name] = threading.Lock() 56 | 57 | self._locks[name].acquire() 58 | self._data[name] = value 59 | self._locks[name].release() 60 | 61 | def __getitem__(self, key): 62 | return self.__getattr__(key) 63 | 64 | def __setitem__(self, key, item): 65 | self._data[key] = item 66 | 67 | def __str__(self): 68 | return str(self._data) 69 | 70 | def keys(self): 71 | """ Gets all of the keys 72 | @returns List A list of all of the keys 73 | """ 74 | return list(self._data.keys()) 75 | 76 | def get_data(self, key, default=None): 77 | """ Gets data from key. If key does not exist, returns default value. 78 | @param key [string] The data key 79 | @param default (None) The default value 80 | @throws KeyError if key not found and default not set 81 | """ 82 | if not key in self._data.keys(): 83 | self._data[key] = default 84 | return self._data[key] 85 | 86 | def set_data(self, key, value): 87 | """ Sets data. 88 | @param key [string] The data key 89 | @param value [*] The data value 90 | """ 91 | self._data[key] = value 92 | 93 | 94 | class NodeStatus(object): 95 | 96 | """ A class for enumerating behavior status 97 | Available statuses: 98 | PENDING: Node hasn't is in pre-run state 99 | ACTIVE: Node is currently executing 100 | SUCCESS: Node has completed successfully 101 | FAIL: Node has completed with errors 102 | CANCEL: Node has been canceled 103 | """ 104 | 105 | PENDING = 0 106 | ACTIVE = 1 107 | SUCCESS = 2 108 | FAIL = 3 109 | CANCEL = 4 110 | 111 | def __init__(self, status=PENDING, text=""): 112 | self.status = status 113 | self.text = text 114 | 115 | def _get_status_str(self): 116 | """ Gets the string of the current status enum""" 117 | status_str = { 118 | self.PENDING: "PENDING", 119 | self.ACTIVE: "ACTIVE", 120 | self.SUCCESS: "SUCCESS", 121 | self.FAIL: "FAIL", 122 | self.CANCEL: "CANCEL" 123 | } 124 | if self.status in status_str: 125 | return status_str[self.status] 126 | return str(self.status) 127 | 128 | def __str__(self): 129 | return str(self._get_status_str() + " " + self.text) 130 | 131 | def __eq__(self, other): 132 | return self.status == other 133 | 134 | def merge(self, status, text): 135 | """ Merges a NodeStatus together. 136 | If the merged status has a greater value than the current status, 137 | the merged status is used. 138 | If the merged status is the same or equal, the text is concatenated. 139 | """ 140 | if status > self.status: 141 | self.status = status 142 | self.text = text 143 | 144 | if status == self.status: 145 | self.text = self.text + "; " + text 146 | 147 | 148 | class Blackboard(object): 149 | 150 | """ A class to contain all (or related) nodedatas. This class allows for 151 | specific node data parameters to be shared amoung nodes. 152 | """ 153 | 154 | def __init__(self): 155 | self._base_memory = {} 156 | self._node_memory = {} 157 | self._node_status = {} 158 | 159 | def _get_node_memory(self, scope): 160 | """ Gets node memory 161 | If memory doesn't exist it will be assigned a default value. 162 | @param scope [uuid] The id of the node. 163 | @returns [dict] The memory. 164 | """ 165 | if not scope in self._node_memory: 166 | self._node_memory[scope] = {'node_data': NodeData(), 167 | 'remapping': {}} 168 | return self._node_memory[scope] 169 | 170 | def _get_node_data(self, node_memory): 171 | """ Gets node data with any remappings. 172 | If node data doesn't exist it will create an empty NodeData object. 173 | @param node_memory [dict] The memory of the node. 174 | @returns [NodeData] The node specific memory. 175 | """ 176 | if not 'node_data' in node_memory: 177 | node_memory['node_data'] = NodeData() 178 | memory = node_memory['node_data'] 179 | logger.debug("node_data: " + str(memory)) 180 | remapping = node_memory['remapping'] 181 | logger.debug("remapping: " + str(remapping)) 182 | 183 | for key in remapping: 184 | (from_scope, from_key) = remapping[key] 185 | try: 186 | value = self.get(from_key, from_scope) 187 | memory[key] = value 188 | except: 189 | pass 190 | 191 | return memory 192 | 193 | def _get_memory(self, scope=None): 194 | """ Gets memory. 195 | If scope is none, it returns global memory 196 | else it returns node memory. 197 | @param scope [uuid] The uuid of the memory. 198 | @returns [dict] The memory. 199 | """ 200 | memory = self._base_memory 201 | logger.debug("base_memory: " + str(memory)) 202 | 203 | if(scope): 204 | memory = self._get_node_memory(scope) 205 | memory = self._get_node_data(memory) 206 | logger.debug(str(scope) + ".node_data: " + str(memory)) 207 | 208 | return memory 209 | 210 | def save(self, key, value, scope=None): 211 | """ Saves a (key, value) pair onto tree_scope/node_scope. 212 | @param key [string] the key of the item. 213 | @param value [*] The value of the item. 214 | @param scope [uuid] (optional) The uuid of the tree. 215 | """ 216 | memory = self._get_memory(scope) 217 | memory[key] = value 218 | 219 | def get(self, key, scope=None): 220 | """ Gets a (key, value) pair from tree_scope/node_scope. 221 | @param key [string] The key to retrieve. 222 | @param scope [uuid] (optional) The uuid of the tree. 223 | @returns The value of the key in the tree_scope/node_scope. 224 | """ 225 | memory = self._get_memory(scope) 226 | return memory[key] 227 | 228 | def add_remapping(self, from_scope, from_key, to_scope, to_key): 229 | """ Add a remapping from one node->key to another node->key. 230 | @param from_scope [uuid] The id of the source node. 231 | @param from_key [string] The key of the source node. 232 | @param to_scope [uuid] The id of the destination node. 233 | @param to_key [string] The key of the destination node. 234 | """ 235 | memory = self._get_node_memory(to_scope) 236 | remap = memory['remapping'] 237 | if to_key in remap: 238 | raise RuntimeError("Can not map to same key twice") 239 | remap[to_key] = (from_scope, from_key) 240 | 241 | def get_memory(self, scope): 242 | """ Gets current nodedata with any remappings. 243 | @param scope [uuid] The id of the scope/node. 244 | """ 245 | return self._get_memory(scope) 246 | 247 | def get_status(self): 248 | """ Gets all of the node status. 249 | """ 250 | return self._node_status 251 | 252 | def get_node_status(self, scope): 253 | """ Gets the status of a specific node. 254 | @param scope [uuid] The id of the node to get. 255 | @returns [NodeStatus] The status of the node. 256 | """ 257 | if not scope in self._node_status: 258 | self._node_status[scope] = NodeStatus() 259 | return self._node_status[scope] 260 | 261 | def set_node_status(self, scope, status): 262 | """ Sets the status of a specific node 263 | @param scope [uuid] The id of the node to set 264 | @param status [NodeStatus] The status to set the node 265 | """ 266 | self._node_status[scope] = status 267 | 268 | def clear_node_status(self): 269 | """ Clears the node_status currently saved. 270 | This is mostly used for display purposes. 271 | """ 272 | self._node_status = {} 273 | 274 | 275 | class Node(object): 276 | 277 | """ Base class for nodes. 278 | This class defines the standard callbacks and data structures for Nodes. 279 | 280 | Callbacks follow a set state machine on each tick() 281 | 282 | PENDING -> configure() -> ACTIVE <-> run() -> CANCEL -> cancel() 283 | | | 284 | -> SUCCESS/FAIL - ---> cleanup() 285 | 286 | The data structure is provided by a Blackboard. By default, each node 287 | creates its own Blackboard, however you may set the blackboard to a 288 | specific one as well so that nodes may share their data. 289 | """ 290 | 291 | def __init__(self, name, blackboard=Blackboard(), run_cb=None, 292 | configure_cb=None, cleanup_cb=None, cancel_cb=None, 293 | *args, **kwargs): 294 | """ Node constructor. 295 | @param name [string] The name of this node. 296 | @param blackboard [Blackboard] The common nodedata container. 297 | @param run_cb [function] The function to call on run. 298 | @param config_cb [function] The function to call on enter. 299 | @param cleanup_cb [function] The function to call on exit. 300 | @param cancel_cb [function] The function to call when canceled. 301 | """ 302 | self._id = uuid.uuid4() 303 | self._name = name 304 | self._force_state = None 305 | self._run_cb = run_cb 306 | self._configure_cb = configure_cb 307 | self._cleanup_cb = cleanup_cb 308 | self._cancel_cb = cancel_cb 309 | self._result = NodeStatus() 310 | self._blackboard = blackboard 311 | 312 | def _configure(self): 313 | """ Configuration performed once before run(). 314 | This is usually used to set up internal variables. 315 | """ 316 | logger.debug( 317 | self._name + "._configure() entering... " + str(self._result)) 318 | if self._configure_cb: 319 | nodedata = self._blackboard.get_memory(self._id) 320 | self._configure_cb(nodedata) 321 | self._result = NodeStatus( 322 | NodeStatus.ACTIVE, "Configured " + self._name) 323 | logger.debug( 324 | self._name + "._configure() exiting.. " + str(self._result)) 325 | 326 | def _cleanup(self): 327 | """ Cleanup performed once after run() returns a termination value. 328 | This is usually used to reset internal variables. 329 | """ 330 | logger.debug( 331 | self._name + "._cleanup() entering... " + str(self._result)) 332 | if self._result == NodeStatus.ACTIVE: 333 | self._cancel() 334 | if self._cleanup_cb: 335 | nodedata = self._blackboard.get_memory(self._id) 336 | self._cleanup_cb(nodedata) 337 | self._force_state = None 338 | self._result = NodeStatus( 339 | NodeStatus.PENDING, "Cleaned up " + self._name) 340 | logger.debug(self._name + "._cleanup() exiting.. " + str(self._result)) 341 | 342 | def _run(self): 343 | """ Evaluates the current node. 344 | @returns [NodeStatus] Outcome status. 345 | """ 346 | logger.info(self._name + "._run() entering... " + str(self._result)) 347 | if self._force_state: 348 | self._result = self._force_state 349 | elif self._run_cb: 350 | nodedata = self._blackboard.get_memory(self._id) 351 | self._result = self._run_cb(nodedata) 352 | else: 353 | raise NotImplementedError('run_cb must be defined.') 354 | if not type(self._result) == NodeStatus: 355 | raise NotImplementedError( 356 | 'Result of run_cb must be a task_behavior_engine.tree.NodeStatus type') 357 | logger.info(self._name + "._run() exiting.. " + str(self._result)) 358 | self._blackboard.set_node_status(self._id, self._result) 359 | logger.info(self._name + " ::SET_NODE_STATUS:: " + str(self._result)) 360 | return self._result 361 | 362 | def _cancel(self): 363 | """ Forces the current state to CANCEL 364 | and calls the cancel callback, if it exists. 365 | """ 366 | logger.debug( 367 | self._name + "._cancel() entering... " + str(self._result)) 368 | self._force_state = NodeStatus( 369 | NodeStatus.CANCEL, "Canceling " + self._name) 370 | nodedata = self._blackboard.get_memory(self._id) 371 | if self._cancel_cb: 372 | self._cancel_cb(nodedata) 373 | self._run() 374 | logger.debug(self._name + "._cancel() exiting.. " + str(self._result)) 375 | 376 | def cancel(self, *args, **kwargs): 377 | """ By default a behavior should just call its internal 378 | cancel when requested. 379 | """ 380 | self._cancel() 381 | 382 | def force(self, status): 383 | """ Forces an execution state. 384 | @param status [int] forced outcome state (must be NodeStatus enum). 385 | """ 386 | logger.debug( 387 | self._name + ".force() entering... " + str(self._force_state)) 388 | self._force_state = NodeStatus(status) 389 | self._force_state.text = "Forcing " + self._name + \ 390 | " to " + self._force_state._get_status_str() 391 | logger.debug( 392 | self._name + ".force() exiting.. " + str(self._force_state)) 393 | 394 | def get_nodedata(self): 395 | """ Return the NodeData of this behavior 396 | @returns [NodeData] The current NodeData 397 | """ 398 | return self._blackboard.get_memory(self._id) 399 | 400 | def set_nodedata(self, key, value): 401 | """ Set a NodeData value for this behavior 402 | @param key [str] The name of the parameter 403 | @param value The value of the parameter 404 | """ 405 | self._blackboard.save(key, value, self._id) 406 | 407 | def get_result(self): 408 | """ Return the result of this behavior. 409 | @returns [NodeStatus] The current result status. 410 | """ 411 | return self._result 412 | 413 | def get_status(self): 414 | """ Get the status of this node. 415 | @returns [NodeStatus] The last completion status of the node. 416 | """ 417 | return self._blackboard.get_node_status(self._id) 418 | 419 | def register_run_cb(self, cb): 420 | """ Register the run_cb. 421 | This will get called when ticked. 422 | @param cb [function] The function to call on run. 423 | """ 424 | self._run_cb = cb 425 | 426 | def register_configure_cb(self, cb): 427 | """ Register the configure_cb. 428 | This will get called on configure. 429 | @param cb [function] The function to call on configure. 430 | """ 431 | self._configure_cb = cb 432 | 433 | def register_cleanup_cb(self, cb): 434 | """ Register the cleanup callback. 435 | This will get called on cleanup. 436 | @param cb [function] The function to call on cleanup. 437 | """ 438 | self._cleanup_cb = cb 439 | 440 | def register_cancel_cb(self, cb): 441 | """ Register the cancel callback. 442 | This will get called if task is canceled. 443 | @param cb [function] The function to call on cancel. 444 | """ 445 | self._cancel_cb = cb 446 | 447 | def set_blackboard(self, blackboard): 448 | """Set the blackboard for this node. 449 | @param blackboard [Blackboard] The blackboard to assign to this node. 450 | """ 451 | self._blackboard = blackboard 452 | 453 | def tick(self, *args, **kwargs): 454 | """Runs the node 455 | """ 456 | logger.debug(self._name + ".tick() entering... " + str(self._result)) 457 | if self._result == NodeStatus.PENDING: 458 | self._configure() 459 | result = self._run() 460 | logger.debug(self._name + ".tick() result: " + str(result)) 461 | if not (self._result == NodeStatus.ACTIVE or self._result == NodeStatus.PENDING): 462 | self._cleanup() 463 | logger.debug(self._name + ".tick() exiting.. " + str(self._result)) 464 | return result 465 | 466 | 467 | class Decorator(Node): 468 | 469 | """ Decorators are nodes that contain one child. Their purpose is to modify 470 | the output of the child node in order to fit into the task logic structure. 471 | """ 472 | 473 | def __init__(self, name, child=None, *args, **kwargs): 474 | super(Decorator, self).__init__(name=name, *args, **kwargs) 475 | self._child = child 476 | 477 | def _configure(self): 478 | """ Configure the child node if this node is configured. 479 | """ 480 | logger.debug( 481 | self._name + "._configure() entering... " + str(self._result)) 482 | if self._child: 483 | self._child._configure() 484 | super(Decorator, self)._configure() 485 | logger.debug( 486 | self._name + "._configure() exiting.. " + str(self._result)) 487 | 488 | def _cleanup(self): 489 | """ Cleanup the child node if this node is cleaned up. 490 | """ 491 | logger.debug( 492 | self._name + "._cleanup() entering... " + str(self._result)) 493 | if self._child: 494 | self._child._cleanup() 495 | super(Decorator, self)._cleanup() 496 | logger.debug(self._name + "._cleanup() exiting.. " + str(self._result)) 497 | 498 | def _cancel(self): 499 | """ Cancel the child node if this node is canceled. 500 | """ 501 | logger.debug( 502 | self._name + "._cancel() entering... " + str(self._result)) 503 | if self._child: 504 | self._child._cancel() 505 | super(Decorator, self)._cancel() 506 | logger.debug(self._name + "._cancel() exiting.. " + str(self._result)) 507 | 508 | def set_child(self, child): 509 | """ Assign the child node. This can also be done at the constructor. 510 | @param child [Node] The child to add to this decorator 511 | """ 512 | self._child = child 513 | 514 | def tick_child(self): 515 | """ Run the child node. 516 | If no child defined, return default status (PENDING) 517 | """ 518 | if self._child: 519 | return self._child.tick() 520 | else: 521 | return NodeStatus() 522 | 523 | 524 | class Behavior(Node): 525 | 526 | """ Behaviors are nodes that contain children. 527 | """ 528 | 529 | def __init__(self, name, *args, **kwargs): 530 | super(Behavior, self).__init__(name=name, *args, **kwargs) 531 | 532 | self._children = [] 533 | self._open_nodes = [] 534 | 535 | def check_unique_child(self, name): 536 | return not name in [child._name for child in self._children] 537 | 538 | def add_child(self, node): 539 | """Add a node to the end behavior tree. 540 | @param node [Node] The node to add. 541 | @throws RuntimeError if the node name is not unique to this behavior. 542 | """ 543 | if self.check_unique_child(node._name): 544 | self._children.append(node) 545 | else: 546 | raise RuntimeError( 547 | "Could not add node %s to %s. Name is not unique." % (node._name, self._name)) 548 | 549 | def remove_child(self, node): 550 | """Remove a node from the behavior tree. 551 | @param node [Node] The node to remove. 552 | @throws ValueError if node not child of behavior. 553 | """ 554 | self._children.remove(node) 555 | 556 | def prepend_child(self, node): 557 | """Add a node to the beginning of the behavior tree. 558 | @param node [Node] The node to prepend. 559 | """ 560 | self._children.insert(0, node) 561 | 562 | def insert_child(self, node, i): 563 | """Insert node into position i of the behavior tree. 564 | @param node [Node] The node to add. 565 | @param i [int] The position of insertion. 566 | """ 567 | self._children.insert(i, node) 568 | 569 | def tick_child(self, child): 570 | """Run a child node 571 | @param child [Node] The child to run 572 | """ 573 | logger.info(child._name + ".tick_child()") 574 | result = child.tick() 575 | if result == NodeStatus.ACTIVE: 576 | if child._id not in self._open_nodes: 577 | logger.info("Adding child " + child._name + " to open_nodes") 578 | self._open_nodes.append(child._id) 579 | if child.get_result() == NodeStatus.PENDING: 580 | if child._id in self._open_nodes: 581 | logger.info( 582 | "Removing child " + child._name + " from open_nodes") 583 | self._open_nodes.remove(child._id) 584 | return result 585 | 586 | def cancel_children(self): 587 | """ Cancel all children currently running 588 | """ 589 | for child in self._children: 590 | self.cancel_child(child) 591 | 592 | def _cancel(self): 593 | """ If a behavior is canceled, cancel all running children. 594 | """ 595 | self.cancel_children() 596 | super(Behavior, self)._cancel() 597 | 598 | def cleanup_children(self): 599 | """ Cleanup all active children 600 | """ 601 | for child in self._children: 602 | if not child.get_result() == NodeStatus.PENDING: 603 | logger.info("Cleaning up child " + child._name) 604 | child._cleanup() 605 | if child._id in self._open_nodes: 606 | logger.info( 607 | "Removing child " + child._name + " from open_nodes") 608 | self._open_nodes.remove(child._id) 609 | 610 | def _cleanup(self): 611 | """ If a behavior finishes, cancel all running children. 612 | """ 613 | self.cancel_children() 614 | self.cleanup_children() 615 | super(Behavior, self)._cleanup() 616 | 617 | def reset_children_status(self): 618 | """ Reset the current node status for all children 619 | """ 620 | for child in self._children: 621 | child._blackboard.set_node_status(child._id, NodeStatus()) 622 | 623 | def cancel_child(self, child): 624 | """ Cancel a particular child. 625 | @param child [Node] The child to cancel 626 | """ 627 | if not child.get_result() == NodeStatus.PENDING: 628 | logger.info("Canceling child " + child._name) 629 | child._cancel() 630 | -------------------------------------------------------------------------------- /test/test_branch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | from nose.tools import assert_equal 16 | 17 | from task_behavior_engine.branch import All 18 | from task_behavior_engine.branch import Any 19 | from task_behavior_engine.branch import First 20 | from task_behavior_engine.branch import Majority 21 | from task_behavior_engine.branch import Progressor 22 | from task_behavior_engine.branch import Random 23 | from task_behavior_engine.branch import Runner 24 | from task_behavior_engine.branch import Selector 25 | from task_behavior_engine.branch import Sequencer 26 | 27 | from task_behavior_engine.node import Continue 28 | from task_behavior_engine.node import Fail 29 | from task_behavior_engine.node import Success 30 | 31 | from task_behavior_engine.tree import Blackboard 32 | from task_behavior_engine.tree import NodeStatus 33 | 34 | 35 | class TestSelector(object): 36 | 37 | def setUp(self): 38 | self.blackboard = Blackboard() 39 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 40 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 41 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 42 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 43 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 44 | 45 | self.SELECT = Selector(name="SELECT") 46 | self.SELECT.set_blackboard(self.blackboard) 47 | self.nd = self.blackboard.get_memory(self.SELECT._id) 48 | 49 | def test_fail(self): 50 | self.SELECT.add_child(self.FAIL1) 51 | self.SELECT.add_child(self.FAIL2) 52 | 53 | result = self.SELECT.tick() 54 | assert_equal(result, NodeStatus.FAIL) 55 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 56 | NodeStatus.FAIL) 57 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 58 | NodeStatus.FAIL) 59 | 60 | def test_success(self): 61 | self.SELECT.add_child(self.FAIL1) 62 | self.SELECT.add_child(self.FAIL2) 63 | self.SELECT.add_child(self.SUCCESS1) 64 | self.SELECT.add_child(self.SUCCESS2) 65 | 66 | result = self.SELECT.tick() 67 | assert_equal(result, NodeStatus.SUCCESS) 68 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 69 | NodeStatus.FAIL) 70 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 71 | NodeStatus.FAIL) 72 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 73 | NodeStatus.SUCCESS) 74 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 75 | NodeStatus.PENDING) 76 | 77 | def test_active(self): 78 | self.SELECT.add_child(self.CONTINUE) 79 | self.SELECT.add_child(self.FAIL1) 80 | self.SELECT.add_child(self.FAIL2) 81 | self.SELECT.add_child(self.SUCCESS1) 82 | self.SELECT.add_child(self.SUCCESS2) 83 | 84 | result = self.SELECT.tick() 85 | assert_equal(result, NodeStatus.ACTIVE) 86 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 87 | NodeStatus.ACTIVE) 88 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 89 | NodeStatus.PENDING) 90 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 91 | NodeStatus.PENDING) 92 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 93 | NodeStatus.PENDING) 94 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 95 | NodeStatus.PENDING) 96 | 97 | def test_cancel(self): 98 | self.SELECT.add_child(self.CONTINUE) 99 | self.SELECT.add_child(self.FAIL1) 100 | self.SELECT.add_child(self.FAIL2) 101 | self.SELECT.add_child(self.SUCCESS1) 102 | self.SELECT.add_child(self.SUCCESS2) 103 | 104 | self.SELECT.tick() 105 | self.SELECT._cancel() 106 | result = self.SELECT.tick() 107 | assert_equal(result, NodeStatus.CANCEL) 108 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 109 | NodeStatus.CANCEL) 110 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 111 | NodeStatus.PENDING) 112 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 113 | NodeStatus.PENDING) 114 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 115 | NodeStatus.PENDING) 116 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 117 | NodeStatus.PENDING) 118 | 119 | def test_force_child(self): 120 | self.SELECT.add_child(self.CONTINUE) 121 | self.SELECT.add_child(self.FAIL1) 122 | self.SELECT.add_child(self.FAIL2) 123 | self.SELECT.add_child(self.SUCCESS1) 124 | self.SELECT.add_child(self.SUCCESS2) 125 | 126 | self.SELECT.tick() 127 | self.CONTINUE.force(NodeStatus.FAIL) 128 | result = self.SELECT.tick() 129 | assert_equal(result, NodeStatus.SUCCESS) 130 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 131 | NodeStatus.FAIL) 132 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 133 | NodeStatus.FAIL) 134 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 135 | NodeStatus.FAIL) 136 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 137 | NodeStatus.SUCCESS) 138 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 139 | NodeStatus.PENDING) 140 | self.blackboard.clear_node_status() 141 | 142 | def test_force_behavior(self): 143 | self.SELECT.add_child(self.CONTINUE) 144 | self.SELECT.add_child(self.FAIL1) 145 | self.SELECT.add_child(self.FAIL2) 146 | self.SELECT.add_child(self.SUCCESS1) 147 | self.SELECT.add_child(self.SUCCESS2) 148 | 149 | self.SELECT.tick() 150 | self.SELECT.force(NodeStatus.FAIL) 151 | result = self.SELECT.tick() 152 | assert_equal(result, NodeStatus.FAIL) 153 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 154 | NodeStatus.CANCEL) 155 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 156 | NodeStatus.PENDING) 157 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 158 | NodeStatus.PENDING) 159 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 160 | NodeStatus.PENDING) 161 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 162 | NodeStatus.PENDING) 163 | self.blackboard.clear_node_status() 164 | 165 | 166 | class TestSequencer(object): 167 | 168 | def setUp(self): 169 | self.blackboard = Blackboard() 170 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 171 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 172 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 173 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 174 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 175 | 176 | self.SEQUENCE = Sequencer("SEQUENCE") 177 | self.SEQUENCE.set_blackboard(self.blackboard) 178 | self.nd = self.blackboard.get_memory(self.SEQUENCE._id) 179 | 180 | def test_success(self): 181 | self.SEQUENCE.add_child(self.SUCCESS1) 182 | self.SEQUENCE.add_child(self.SUCCESS2) 183 | 184 | result = self.SEQUENCE.tick() 185 | assert_equal(result, NodeStatus.SUCCESS) 186 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 187 | NodeStatus.SUCCESS) 188 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 189 | NodeStatus.SUCCESS) 190 | 191 | def test_fail(self): 192 | self.SEQUENCE.add_child(self.SUCCESS1) 193 | self.SEQUENCE.add_child(self.SUCCESS2) 194 | self.SEQUENCE.add_child(self.FAIL1) 195 | self.SEQUENCE.add_child(self.FAIL2) 196 | 197 | result = self.SEQUENCE.tick() 198 | assert_equal(result, NodeStatus.FAIL) 199 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 200 | NodeStatus.SUCCESS) 201 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 202 | NodeStatus.SUCCESS) 203 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 204 | NodeStatus.FAIL) 205 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 206 | NodeStatus.PENDING) 207 | 208 | def test_active(self): 209 | self.SEQUENCE.add_child(self.CONTINUE) 210 | self.SEQUENCE.add_child(self.SUCCESS1) 211 | self.SEQUENCE.add_child(self.SUCCESS2) 212 | self.SEQUENCE.add_child(self.FAIL1) 213 | self.SEQUENCE.add_child(self.FAIL2) 214 | 215 | result = self.SEQUENCE.tick() 216 | assert_equal(result, NodeStatus.ACTIVE) 217 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 218 | NodeStatus.ACTIVE) 219 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 220 | NodeStatus.PENDING) 221 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 222 | NodeStatus.PENDING) 223 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 224 | NodeStatus.PENDING) 225 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 226 | NodeStatus.PENDING) 227 | 228 | def test_cancel(self): 229 | self.SEQUENCE.add_child(self.CONTINUE) 230 | self.SEQUENCE.add_child(self.SUCCESS1) 231 | self.SEQUENCE.add_child(self.SUCCESS2) 232 | self.SEQUENCE.add_child(self.FAIL1) 233 | self.SEQUENCE.add_child(self.FAIL2) 234 | 235 | self.SEQUENCE.tick() 236 | self.SEQUENCE._cancel() 237 | result = self.SEQUENCE.tick() 238 | assert_equal(result, NodeStatus.CANCEL) 239 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 240 | NodeStatus.CANCEL) 241 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 242 | NodeStatus.PENDING) 243 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 244 | NodeStatus.PENDING) 245 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 246 | NodeStatus.PENDING) 247 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 248 | NodeStatus.PENDING) 249 | 250 | def test_force_child(self): 251 | self.SEQUENCE.add_child(self.CONTINUE) 252 | self.SEQUENCE.add_child(self.SUCCESS1) 253 | self.SEQUENCE.add_child(self.SUCCESS2) 254 | self.SEQUENCE.add_child(self.FAIL1) 255 | self.SEQUENCE.add_child(self.FAIL2) 256 | 257 | self.SEQUENCE.tick() 258 | self.CONTINUE.force(NodeStatus.SUCCESS) 259 | result = self.SEQUENCE.tick() 260 | assert_equal(result, NodeStatus.FAIL) 261 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 262 | NodeStatus.SUCCESS) 263 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 264 | NodeStatus.SUCCESS) 265 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 266 | NodeStatus.SUCCESS) 267 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 268 | NodeStatus.FAIL) 269 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 270 | NodeStatus.PENDING) 271 | 272 | def test_force_behavior(self): 273 | self.SEQUENCE.add_child(self.CONTINUE) 274 | self.SEQUENCE.add_child(self.SUCCESS1) 275 | self.SEQUENCE.add_child(self.SUCCESS2) 276 | self.SEQUENCE.add_child(self.FAIL1) 277 | self.SEQUENCE.add_child(self.FAIL2) 278 | 279 | self.SEQUENCE.tick() 280 | self.SEQUENCE.force(NodeStatus.SUCCESS) 281 | result = self.SEQUENCE.tick() 282 | assert_equal(result, NodeStatus.SUCCESS) 283 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 284 | NodeStatus.CANCEL) 285 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 286 | NodeStatus.PENDING) 287 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 288 | NodeStatus.PENDING) 289 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 290 | NodeStatus.PENDING) 291 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 292 | NodeStatus.PENDING) 293 | 294 | 295 | class TestRunner(object): 296 | 297 | def setUp(self): 298 | self.blackboard = Blackboard() 299 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 300 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 301 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 302 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 303 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 304 | 305 | self.RUNNER = Runner("RUNNER") 306 | self.RUNNER.set_blackboard(self.blackboard) 307 | self.nd = self.blackboard.get_memory(self.RUNNER._id) 308 | 309 | def test_success(self): 310 | self.RUNNER.add_child(self.SUCCESS1) 311 | self.RUNNER.add_child(self.FAIL1) 312 | self.RUNNER.add_child(self.SUCCESS2) 313 | self.RUNNER.add_child(self.FAIL2) 314 | 315 | result = self.RUNNER.tick() 316 | assert_equal(result, NodeStatus.SUCCESS) 317 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 318 | NodeStatus.SUCCESS) 319 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 320 | NodeStatus.FAIL) 321 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 322 | NodeStatus.SUCCESS) 323 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 324 | NodeStatus.FAIL) 325 | 326 | def test_active(self): 327 | self.RUNNER.add_child(self.SUCCESS1) 328 | self.RUNNER.add_child(self.FAIL1) 329 | self.RUNNER.add_child(self.CONTINUE) 330 | self.RUNNER.add_child(self.SUCCESS2) 331 | self.RUNNER.add_child(self.FAIL2) 332 | 333 | result = self.RUNNER.tick() 334 | assert_equal(result, NodeStatus.ACTIVE) 335 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 336 | NodeStatus.SUCCESS) 337 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 338 | NodeStatus.FAIL) 339 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 340 | NodeStatus.ACTIVE) 341 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 342 | NodeStatus.PENDING) 343 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 344 | NodeStatus.PENDING) 345 | 346 | def test_cancel(self): 347 | self.RUNNER.add_child(self.SUCCESS1) 348 | self.RUNNER.add_child(self.FAIL1) 349 | self.RUNNER.add_child(self.CONTINUE) 350 | self.RUNNER.add_child(self.SUCCESS2) 351 | self.RUNNER.add_child(self.FAIL2) 352 | 353 | self.RUNNER.tick() 354 | self.RUNNER._cancel() 355 | result = self.RUNNER.tick() 356 | assert_equal(result, NodeStatus.CANCEL) 357 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 358 | NodeStatus.SUCCESS) 359 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 360 | NodeStatus.FAIL) 361 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 362 | NodeStatus.CANCEL) 363 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 364 | NodeStatus.PENDING) 365 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 366 | NodeStatus.PENDING) 367 | 368 | def test_force_child(self): 369 | self.RUNNER.add_child(self.SUCCESS1) 370 | self.RUNNER.add_child(self.FAIL1) 371 | self.RUNNER.add_child(self.CONTINUE) 372 | self.RUNNER.add_child(self.SUCCESS2) 373 | self.RUNNER.add_child(self.FAIL2) 374 | 375 | self.RUNNER.tick() 376 | self.CONTINUE.force(NodeStatus.SUCCESS) 377 | result = self.RUNNER.tick() 378 | assert_equal(result, NodeStatus.SUCCESS) 379 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 380 | NodeStatus.SUCCESS) 381 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 382 | NodeStatus.FAIL) 383 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 384 | NodeStatus.SUCCESS) 385 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 386 | NodeStatus.SUCCESS) 387 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 388 | NodeStatus.FAIL) 389 | 390 | def test_force_behavior(self): 391 | self.RUNNER.add_child(self.SUCCESS1) 392 | self.RUNNER.add_child(self.FAIL1) 393 | self.RUNNER.add_child(self.CONTINUE) 394 | self.RUNNER.add_child(self.SUCCESS2) 395 | self.RUNNER.add_child(self.FAIL2) 396 | 397 | self.RUNNER.tick() 398 | self.RUNNER.force(NodeStatus.FAIL) 399 | result = self.RUNNER.tick() 400 | assert_equal(result, NodeStatus.FAIL) 401 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 402 | NodeStatus.SUCCESS) 403 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 404 | NodeStatus.FAIL) 405 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 406 | NodeStatus.CANCEL) 407 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 408 | NodeStatus.PENDING) 409 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 410 | NodeStatus.PENDING) 411 | 412 | 413 | class TestAny(object): 414 | 415 | def setUp(self): 416 | self.blackboard = Blackboard() 417 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 418 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 419 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 420 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 421 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 422 | 423 | self.ANY = Any("ANY") 424 | self.ANY.set_blackboard(self.blackboard) 425 | self.nd = self.blackboard.get_memory(self.ANY._id) 426 | 427 | def test_fail(self): 428 | self.ANY.add_child(self.FAIL1) 429 | self.ANY.add_child(self.FAIL2) 430 | 431 | result = self.ANY.tick() 432 | assert_equal(result, NodeStatus.FAIL) 433 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 434 | NodeStatus.FAIL) 435 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 436 | NodeStatus.FAIL) 437 | 438 | def test_success(self): 439 | self.ANY.add_child(self.FAIL1) 440 | self.ANY.add_child(self.FAIL2) 441 | self.ANY.add_child(self.SUCCESS1) 442 | self.ANY.add_child(self.SUCCESS2) 443 | 444 | result = self.ANY.tick() 445 | assert_equal(result, NodeStatus.SUCCESS) 446 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 447 | NodeStatus.FAIL) 448 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 449 | NodeStatus.FAIL) 450 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 451 | NodeStatus.SUCCESS) 452 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 453 | NodeStatus.PENDING) 454 | 455 | def test_active(self): 456 | self.ANY.add_child(self.FAIL1) 457 | self.ANY.add_child(self.CONTINUE) 458 | self.ANY.add_child(self.FAIL2) 459 | 460 | result = self.ANY.tick() 461 | assert_equal(result, NodeStatus.ACTIVE) 462 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 463 | NodeStatus.FAIL) 464 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 465 | NodeStatus.ACTIVE) 466 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 467 | NodeStatus.FAIL) 468 | 469 | def test_cancel(self): 470 | self.ANY.add_child(self.FAIL1) 471 | self.ANY.add_child(self.CONTINUE) 472 | self.ANY.add_child(self.FAIL2) 473 | 474 | self.ANY.tick() 475 | self.ANY._cancel() 476 | result = self.ANY.tick() 477 | assert_equal(result, NodeStatus.CANCEL) 478 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 479 | NodeStatus.FAIL) 480 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 481 | NodeStatus.CANCEL) 482 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 483 | NodeStatus.FAIL) 484 | 485 | def test_force_child(self): 486 | self.ANY.add_child(self.FAIL1) 487 | self.ANY.add_child(self.CONTINUE) 488 | self.ANY.add_child(self.FAIL2) 489 | 490 | self.ANY.tick() 491 | self.CONTINUE.force(NodeStatus.SUCCESS) 492 | result = self.ANY.tick() 493 | assert_equal(result, NodeStatus.SUCCESS) 494 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 495 | NodeStatus.FAIL) 496 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 497 | NodeStatus.SUCCESS) 498 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 499 | NodeStatus.FAIL) 500 | 501 | def test_force_behavior(self): 502 | self.ANY.add_child(self.FAIL1) 503 | self.ANY.add_child(self.CONTINUE) 504 | self.ANY.add_child(self.FAIL2) 505 | 506 | self.ANY.tick() 507 | self.ANY.force(NodeStatus.FAIL) 508 | result = self.ANY.tick() 509 | assert_equal(result, NodeStatus.FAIL) 510 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 511 | NodeStatus.FAIL) 512 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 513 | NodeStatus.CANCEL) 514 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 515 | NodeStatus.FAIL) 516 | 517 | 518 | class TestAll(object): 519 | 520 | def setUp(self): 521 | self.blackboard = Blackboard() 522 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 523 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 524 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 525 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 526 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 527 | 528 | self.ALL = All("ALL") 529 | self.ALL.set_blackboard(self.blackboard) 530 | self.nd = self.blackboard.get_memory(self.ALL._id) 531 | 532 | def test_success(self): 533 | self.ALL.add_child(self.SUCCESS1) 534 | self.ALL.add_child(self.SUCCESS2) 535 | 536 | result = self.ALL.tick() 537 | assert_equal(result, NodeStatus.SUCCESS) 538 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 539 | NodeStatus.SUCCESS) 540 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 541 | NodeStatus.SUCCESS) 542 | 543 | def test_fail(self): 544 | self.ALL.add_child(self.SUCCESS1) 545 | self.ALL.add_child(self.SUCCESS2) 546 | self.ALL.add_child(self.FAIL1) 547 | self.ALL.add_child(self.FAIL2) 548 | 549 | result = self.ALL.tick() 550 | assert_equal(result, NodeStatus.FAIL) 551 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 552 | NodeStatus.SUCCESS) 553 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 554 | NodeStatus.SUCCESS) 555 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 556 | NodeStatus.FAIL) 557 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 558 | NodeStatus.PENDING) 559 | 560 | def test_active(self): 561 | self.ALL.add_child(self.SUCCESS1) 562 | self.ALL.add_child(self.CONTINUE) 563 | self.ALL.add_child(self.SUCCESS2) 564 | 565 | result = self.ALL.tick() 566 | assert_equal(result, NodeStatus.ACTIVE) 567 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 568 | NodeStatus.SUCCESS) 569 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 570 | NodeStatus.ACTIVE) 571 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 572 | NodeStatus.SUCCESS) 573 | 574 | def test_cancel(self): 575 | self.ALL.add_child(self.SUCCESS1) 576 | self.ALL.add_child(self.CONTINUE) 577 | self.ALL.add_child(self.SUCCESS2) 578 | 579 | self.ALL.tick() 580 | self.ALL._cancel() 581 | result = self.ALL.tick() 582 | assert_equal(result, NodeStatus.CANCEL) 583 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 584 | NodeStatus.SUCCESS) 585 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 586 | NodeStatus.CANCEL) 587 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 588 | NodeStatus.SUCCESS) 589 | 590 | def test_force_child(self): 591 | self.ALL.add_child(self.SUCCESS1) 592 | self.ALL.add_child(self.CONTINUE) 593 | self.ALL.add_child(self.SUCCESS2) 594 | 595 | self.ALL.tick() 596 | self.CONTINUE.force(NodeStatus.SUCCESS) 597 | result = self.ALL.tick() 598 | assert_equal(result, NodeStatus.SUCCESS) 599 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 600 | NodeStatus.SUCCESS) 601 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 602 | NodeStatus.SUCCESS) 603 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 604 | NodeStatus.SUCCESS) 605 | 606 | def test_force_behavior(self): 607 | self.ALL.add_child(self.SUCCESS1) 608 | self.ALL.add_child(self.CONTINUE) 609 | self.ALL.add_child(self.SUCCESS2) 610 | 611 | self.ALL.tick() 612 | self.ALL.force(NodeStatus.FAIL) 613 | result = self.ALL.tick() 614 | assert_equal(result, NodeStatus.FAIL) 615 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 616 | NodeStatus.SUCCESS) 617 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 618 | NodeStatus.CANCEL) 619 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 620 | NodeStatus.SUCCESS) 621 | self.blackboard.clear_node_status() 622 | 623 | 624 | class TestRandom(object): 625 | 626 | def setUp(self): 627 | self.blackboard = Blackboard() 628 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 629 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 630 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 631 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 632 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 633 | 634 | self.RANDOM = Random("RANDOM") 635 | self.RANDOM.set_blackboard(self.blackboard) 636 | self.nd = self.blackboard.get_memory(self.RANDOM._id) 637 | 638 | def test_empty(self): 639 | # if empty, configuration should succeed but set child to None 640 | self.RANDOM._configure() 641 | assert_equal(self.RANDOM.child, None) 642 | 643 | # if ticked, random should report success if no children 644 | result = self.RANDOM.tick() 645 | assert_equal(self.RANDOM.child, None) 646 | assert_equal(result, NodeStatus.SUCCESS) 647 | 648 | def test_success(self): 649 | self.RANDOM.add_child(self.SUCCESS1) 650 | self.RANDOM.add_child(self.FAIL1) 651 | self.RANDOM.add_child(self.CONTINUE) 652 | 653 | self.RANDOM._configure() 654 | assert_equal(self.RANDOM.child in self.RANDOM._children, True) 655 | while (self.RANDOM.child != self.SUCCESS1): 656 | self.RANDOM._configure() 657 | 658 | result = self.RANDOM.tick() 659 | assert_equal(result, NodeStatus.SUCCESS) 660 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 661 | NodeStatus.SUCCESS) 662 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 663 | NodeStatus.PENDING) 664 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 665 | NodeStatus.PENDING) 666 | 667 | def test_fail(self): 668 | self.RANDOM.add_child(self.SUCCESS1) 669 | self.RANDOM.add_child(self.FAIL1) 670 | self.RANDOM.add_child(self.CONTINUE) 671 | 672 | self.RANDOM._configure() 673 | assert_equal(self.RANDOM.child in self.RANDOM._children, True) 674 | while (self.RANDOM.child != self.FAIL1): 675 | self.RANDOM._configure() 676 | 677 | result = self.RANDOM.tick() 678 | assert_equal(result, NodeStatus.FAIL) 679 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 680 | NodeStatus.PENDING) 681 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 682 | NodeStatus.FAIL) 683 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 684 | NodeStatus.PENDING) 685 | 686 | def test_active(self): 687 | self.RANDOM.add_child(self.SUCCESS1) 688 | self.RANDOM.add_child(self.FAIL1) 689 | self.RANDOM.add_child(self.CONTINUE) 690 | 691 | self.RANDOM._configure() 692 | assert_equal(self.RANDOM.child in self.RANDOM._children, True) 693 | while (self.RANDOM.child != self.CONTINUE): 694 | self.RANDOM._configure() 695 | 696 | result = self.RANDOM.tick() 697 | assert_equal(result, NodeStatus.ACTIVE) 698 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 699 | NodeStatus.PENDING) 700 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 701 | NodeStatus.PENDING) 702 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 703 | NodeStatus.ACTIVE) 704 | 705 | def test_cancel(self): 706 | self.RANDOM.add_child(self.SUCCESS1) 707 | self.RANDOM.add_child(self.FAIL1) 708 | self.RANDOM.add_child(self.CONTINUE) 709 | 710 | self.RANDOM._configure() 711 | assert_equal(self.RANDOM.child in self.RANDOM._children, True) 712 | while (self.RANDOM.child != self.CONTINUE): 713 | self.RANDOM._configure() 714 | 715 | self.RANDOM.tick() 716 | self.RANDOM._cancel() 717 | result = self.RANDOM.tick() 718 | assert_equal(result, NodeStatus.CANCEL) 719 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 720 | NodeStatus.PENDING) 721 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 722 | NodeStatus.PENDING) 723 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 724 | NodeStatus.CANCEL) 725 | 726 | def test_force_child(self): 727 | self.RANDOM.add_child(self.SUCCESS1) 728 | self.RANDOM.add_child(self.FAIL1) 729 | self.RANDOM.add_child(self.CONTINUE) 730 | 731 | self.RANDOM._configure() 732 | self.RANDOM.child.force(NodeStatus.SUCCESS) 733 | result = self.RANDOM.tick() 734 | assert_equal(result, NodeStatus.SUCCESS) 735 | assert_equal(self.blackboard.get_node_status(self.RANDOM.child._id), 736 | NodeStatus.SUCCESS) 737 | 738 | def test_force_behavior(self): 739 | self.RANDOM.add_child(self.SUCCESS1) 740 | self.RANDOM.add_child(self.FAIL1) 741 | self.RANDOM.add_child(self.CONTINUE) 742 | 743 | self.RANDOM._configure() 744 | self.RANDOM.force(NodeStatus.FAIL) 745 | result = self.RANDOM.tick() 746 | assert_equal(result, NodeStatus.FAIL) 747 | assert_equal(self.blackboard.get_node_status(self.RANDOM.child._id), 748 | NodeStatus.PENDING) 749 | 750 | 751 | class TestProgressor(object): 752 | 753 | def setUp(self): 754 | self.blackboard = Blackboard() 755 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 756 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 757 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 758 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 759 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 760 | self.CONTINUE2 = Continue(name="CONTINUE2", blackboard=self.blackboard) 761 | 762 | self.PROGRESS = Progressor("PROGRESS") 763 | self.PROGRESS.set_blackboard(self.blackboard) 764 | self.nd = self.blackboard.get_memory(self.PROGRESS._id) 765 | 766 | def test_success(self): 767 | self.PROGRESS.add_child(self.SUCCESS1) 768 | self.PROGRESS.add_child(self.SUCCESS2) 769 | 770 | result = self.PROGRESS.tick() 771 | assert_equal(result, NodeStatus.SUCCESS) 772 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 773 | NodeStatus.SUCCESS) 774 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 775 | NodeStatus.SUCCESS) 776 | 777 | def test_fail(self): 778 | self.PROGRESS.add_child(self.SUCCESS1) 779 | self.PROGRESS.add_child(self.SUCCESS2) 780 | self.PROGRESS.add_child(self.FAIL1) 781 | self.PROGRESS.add_child(self.FAIL2) 782 | 783 | result = self.PROGRESS.tick() 784 | assert_equal(result, NodeStatus.FAIL) 785 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 786 | NodeStatus.SUCCESS) 787 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 788 | NodeStatus.SUCCESS) 789 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 790 | NodeStatus.FAIL) 791 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 792 | NodeStatus.PENDING) 793 | 794 | def test_active(self): 795 | self.PROGRESS.add_child(self.SUCCESS1) 796 | self.PROGRESS.add_child(self.CONTINUE) 797 | self.PROGRESS.add_child(self.SUCCESS2) 798 | self.PROGRESS.add_child(self.FAIL1) 799 | self.PROGRESS.add_child(self.FAIL2) 800 | 801 | result = self.PROGRESS.tick() 802 | assert_equal(result, NodeStatus.ACTIVE) 803 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 804 | NodeStatus.SUCCESS) 805 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 806 | NodeStatus.ACTIVE) 807 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 808 | NodeStatus.PENDING) 809 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 810 | NodeStatus.PENDING) 811 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 812 | NodeStatus.PENDING) 813 | 814 | def test_cancel(self): 815 | self.PROGRESS.add_child(self.SUCCESS1) 816 | self.PROGRESS.add_child(self.CONTINUE) 817 | self.PROGRESS.add_child(self.SUCCESS2) 818 | self.PROGRESS.add_child(self.FAIL1) 819 | self.PROGRESS.add_child(self.FAIL2) 820 | 821 | self.PROGRESS.tick() 822 | self.PROGRESS._cancel() 823 | result = self.PROGRESS.tick() 824 | assert_equal(result, NodeStatus.CANCEL) 825 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 826 | NodeStatus.SUCCESS) 827 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 828 | NodeStatus.CANCEL) 829 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 830 | NodeStatus.PENDING) 831 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 832 | NodeStatus.PENDING) 833 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 834 | NodeStatus.PENDING) 835 | 836 | def test_force_child(self): 837 | self.PROGRESS.add_child(self.SUCCESS1) 838 | self.PROGRESS.add_child(self.CONTINUE) 839 | self.PROGRESS.add_child(self.CONTINUE2) 840 | self.PROGRESS.add_child(self.FAIL1) 841 | self.PROGRESS.add_child(self.FAIL2) 842 | 843 | self.PROGRESS.tick() 844 | self.CONTINUE.force(NodeStatus.SUCCESS) 845 | result = self.PROGRESS.tick() 846 | assert_equal(result, NodeStatus.ACTIVE) 847 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 848 | NodeStatus.SUCCESS) 849 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 850 | NodeStatus.SUCCESS) 851 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 852 | NodeStatus.ACTIVE) 853 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 854 | NodeStatus.PENDING) 855 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 856 | NodeStatus.PENDING) 857 | 858 | self.CONTINUE2.force(NodeStatus.SUCCESS) 859 | result = self.PROGRESS.tick() 860 | assert_equal(result, NodeStatus.FAIL) 861 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 862 | NodeStatus.SUCCESS) 863 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 864 | NodeStatus.SUCCESS) 865 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 866 | NodeStatus.SUCCESS) 867 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 868 | NodeStatus.FAIL) 869 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 870 | NodeStatus.PENDING) 871 | 872 | def test_force_behavior(self): 873 | self.PROGRESS.add_child(self.SUCCESS1) 874 | self.PROGRESS.add_child(self.CONTINUE) 875 | self.PROGRESS.add_child(self.CONTINUE2) 876 | self.PROGRESS.add_child(self.FAIL1) 877 | self.PROGRESS.add_child(self.FAIL2) 878 | 879 | self.PROGRESS.tick() 880 | self.PROGRESS.force(NodeStatus.SUCCESS) 881 | result = self.PROGRESS.tick() 882 | assert_equal(result, NodeStatus.SUCCESS) 883 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 884 | NodeStatus.SUCCESS) 885 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 886 | NodeStatus.CANCEL) 887 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 888 | NodeStatus.PENDING) 889 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 890 | NodeStatus.PENDING) 891 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 892 | NodeStatus.PENDING) 893 | 894 | 895 | class TestMajority(object): 896 | 897 | def setUp(self): 898 | self.blackboard = Blackboard() 899 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 900 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 901 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 902 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 903 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 904 | self.CONTINUE2 = Continue(name="CONTINUE2", blackboard=self.blackboard) 905 | 906 | self.MAJORITY = Majority("MAJORITY") 907 | self.MAJORITY.set_blackboard(self.blackboard) 908 | self.nd = self.blackboard.get_memory(self.MAJORITY._id) 909 | 910 | def test_success(self): 911 | self.MAJORITY.add_child(self.SUCCESS1) 912 | self.MAJORITY.add_child(self.SUCCESS2) 913 | 914 | result = self.MAJORITY.tick() 915 | assert_equal(result.status, NodeStatus.SUCCESS) 916 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 917 | NodeStatus.SUCCESS) 918 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 919 | NodeStatus.PENDING) 920 | 921 | def test_fail(self): 922 | self.MAJORITY.add_child(self.FAIL1) 923 | self.MAJORITY.add_child(self.FAIL2) 924 | 925 | result = self.MAJORITY.tick() 926 | assert_equal(result, NodeStatus.FAIL) 927 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 928 | NodeStatus.FAIL) 929 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 930 | NodeStatus.FAIL) 931 | 932 | self.MAJORITY.add_child(self.SUCCESS1) 933 | result = self.MAJORITY.tick() 934 | assert_equal(result, NodeStatus.FAIL) 935 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 936 | NodeStatus.FAIL) 937 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 938 | NodeStatus.FAIL) 939 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 940 | NodeStatus.PENDING) 941 | 942 | def test_active(self): 943 | self.MAJORITY.add_child(self.CONTINUE) 944 | self.MAJORITY.add_child(self.SUCCESS1) 945 | self.MAJORITY.add_child(self.SUCCESS2) 946 | self.MAJORITY.add_child(self.FAIL1) 947 | self.MAJORITY.add_child(self.FAIL2) 948 | 949 | result = self.MAJORITY.tick() 950 | assert_equal(result, NodeStatus.ACTIVE) 951 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 952 | NodeStatus.ACTIVE) 953 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 954 | NodeStatus.SUCCESS) 955 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 956 | NodeStatus.SUCCESS) 957 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 958 | NodeStatus.FAIL) 959 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 960 | NodeStatus.FAIL) 961 | 962 | def test_cancel(self): 963 | self.MAJORITY.add_child(self.CONTINUE) 964 | self.MAJORITY.add_child(self.SUCCESS1) 965 | self.MAJORITY.add_child(self.SUCCESS2) 966 | self.MAJORITY.add_child(self.FAIL1) 967 | self.MAJORITY.add_child(self.FAIL2) 968 | 969 | self.MAJORITY.tick() 970 | self.MAJORITY._cancel() 971 | result = self.MAJORITY.tick() 972 | assert_equal(result, NodeStatus.CANCEL) 973 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 974 | NodeStatus.CANCEL) 975 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 976 | NodeStatus.SUCCESS) 977 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 978 | NodeStatus.SUCCESS) 979 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 980 | NodeStatus.FAIL) 981 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 982 | NodeStatus.FAIL) 983 | 984 | def test_force_child(self): 985 | self.MAJORITY.add_child(self.CONTINUE) 986 | self.MAJORITY.add_child(self.SUCCESS1) 987 | self.MAJORITY.add_child(self.SUCCESS2) 988 | self.MAJORITY.add_child(self.FAIL1) 989 | self.MAJORITY.add_child(self.FAIL2) 990 | 991 | self.MAJORITY.tick() 992 | self.CONTINUE.force(NodeStatus.SUCCESS) 993 | result = self.MAJORITY.tick() 994 | assert_equal(result, NodeStatus.SUCCESS) 995 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 996 | NodeStatus.SUCCESS) 997 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 998 | NodeStatus.SUCCESS) 999 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 1000 | NodeStatus.SUCCESS) 1001 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 1002 | NodeStatus.FAIL) 1003 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 1004 | NodeStatus.FAIL) 1005 | 1006 | self.CONTINUE.force(NodeStatus.FAIL) 1007 | result = self.MAJORITY.tick() 1008 | assert_equal(result, NodeStatus.FAIL) 1009 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 1010 | NodeStatus.FAIL) 1011 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 1012 | NodeStatus.SUCCESS) 1013 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 1014 | NodeStatus.SUCCESS) 1015 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 1016 | NodeStatus.FAIL) 1017 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 1018 | NodeStatus.FAIL) 1019 | 1020 | def test_force_behavior(self): 1021 | self.MAJORITY.add_child(self.CONTINUE) 1022 | self.MAJORITY.add_child(self.SUCCESS1) 1023 | self.MAJORITY.add_child(self.SUCCESS2) 1024 | self.MAJORITY.add_child(self.FAIL1) 1025 | self.MAJORITY.add_child(self.FAIL2) 1026 | 1027 | self.MAJORITY.tick() 1028 | self.MAJORITY.force(NodeStatus.SUCCESS) 1029 | result = self.MAJORITY.tick() 1030 | assert_equal(result, NodeStatus.SUCCESS) 1031 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 1032 | NodeStatus.CANCEL) 1033 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 1034 | NodeStatus.SUCCESS) 1035 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 1036 | NodeStatus.SUCCESS) 1037 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 1038 | NodeStatus.FAIL) 1039 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 1040 | NodeStatus.FAIL) 1041 | 1042 | 1043 | class TestFirst(object): 1044 | 1045 | def setUp(self): 1046 | self.blackboard = Blackboard() 1047 | self.FAIL1 = Fail(name="FAIL1", blackboard=self.blackboard) 1048 | self.FAIL2 = Fail(name="FAIL2", blackboard=self.blackboard) 1049 | self.SUCCESS1 = Success(name="SUCCESS1", blackboard=self.blackboard) 1050 | self.SUCCESS2 = Success(name="SUCCESS2", blackboard=self.blackboard) 1051 | self.CONTINUE1 = Continue(name="CONTINUE1", blackboard=self.blackboard) 1052 | self.CONTINUE2 = Continue(name="CONTINUE2", blackboard=self.blackboard) 1053 | 1054 | self.FIRST = First("FIRST") 1055 | self.FIRST.set_blackboard(self.blackboard) 1056 | self.nd = self.blackboard.get_memory(self.FIRST._id) 1057 | 1058 | def test_success(self): 1059 | self.FIRST.add_child(self.SUCCESS1) 1060 | self.FIRST.add_child(self.SUCCESS2) 1061 | 1062 | result = self.FIRST.tick() 1063 | assert_equal(result.status, NodeStatus.SUCCESS) 1064 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 1065 | NodeStatus.SUCCESS) 1066 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 1067 | NodeStatus.PENDING) 1068 | 1069 | def test_fail(self): 1070 | self.FIRST.add_child(self.FAIL1) 1071 | self.FIRST.add_child(self.FAIL2) 1072 | 1073 | result = self.FIRST.tick() 1074 | assert_equal(result, NodeStatus.FAIL) 1075 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 1076 | NodeStatus.FAIL) 1077 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 1078 | NodeStatus.PENDING) 1079 | 1080 | def test_active(self): 1081 | self.FIRST.add_child(self.CONTINUE1) 1082 | self.FIRST.add_child(self.CONTINUE2) 1083 | 1084 | result = self.FIRST.tick() 1085 | assert_equal(result, NodeStatus.ACTIVE) 1086 | assert_equal(self.blackboard.get_node_status(self.CONTINUE1._id), 1087 | NodeStatus.ACTIVE) 1088 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 1089 | NodeStatus.ACTIVE) 1090 | 1091 | self.FIRST.add_child(self.SUCCESS1) 1092 | self.FIRST.add_child(self.SUCCESS2) 1093 | self.FIRST.add_child(self.FAIL1) 1094 | self.FIRST.add_child(self.FAIL2) 1095 | 1096 | result = self.FIRST.tick() 1097 | assert_equal(result, NodeStatus.SUCCESS) 1098 | assert_equal(self.blackboard.get_node_status(self.CONTINUE1._id), 1099 | NodeStatus.CANCEL) 1100 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 1101 | NodeStatus.CANCEL) 1102 | assert_equal(self.blackboard.get_node_status(self.SUCCESS1._id), 1103 | NodeStatus.SUCCESS) 1104 | assert_equal(self.blackboard.get_node_status(self.SUCCESS2._id), 1105 | NodeStatus.PENDING) 1106 | assert_equal(self.blackboard.get_node_status(self.FAIL1._id), 1107 | NodeStatus.PENDING) 1108 | assert_equal(self.blackboard.get_node_status(self.FAIL2._id), 1109 | NodeStatus.PENDING) 1110 | 1111 | def test_cancel(self): 1112 | self.FIRST.add_child(self.CONTINUE1) 1113 | self.FIRST.add_child(self.CONTINUE2) 1114 | 1115 | self.FIRST.tick() 1116 | self.FIRST._cancel() 1117 | result = self.FIRST.tick() 1118 | assert_equal(result, NodeStatus.CANCEL) 1119 | assert_equal(self.blackboard.get_node_status(self.CONTINUE1._id), 1120 | NodeStatus.CANCEL) 1121 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 1122 | NodeStatus.CANCEL) 1123 | 1124 | def test_force_child(self): 1125 | self.FIRST.add_child(self.CONTINUE1) 1126 | self.FIRST.add_child(self.CONTINUE2) 1127 | 1128 | self.FIRST.tick() 1129 | self.CONTINUE1.force(NodeStatus.SUCCESS) 1130 | result = self.FIRST.tick() 1131 | assert_equal(result, NodeStatus.SUCCESS) 1132 | assert_equal(self.blackboard.get_node_status(self.CONTINUE1._id), 1133 | NodeStatus.SUCCESS) 1134 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 1135 | NodeStatus.CANCEL) 1136 | 1137 | self.CONTINUE1.force(NodeStatus.FAIL) 1138 | result = self.FIRST.tick() 1139 | assert_equal(result, NodeStatus.FAIL) 1140 | assert_equal(self.blackboard.get_node_status(self.CONTINUE1._id), 1141 | NodeStatus.FAIL) 1142 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 1143 | NodeStatus.CANCEL) 1144 | 1145 | def test_force_behavior(self): 1146 | self.FIRST.add_child(self.CONTINUE1) 1147 | self.FIRST.add_child(self.CONTINUE2) 1148 | 1149 | self.FIRST.tick() 1150 | self.FIRST.force(NodeStatus.SUCCESS) 1151 | result = self.FIRST.tick() 1152 | assert_equal(result, NodeStatus.SUCCESS) 1153 | assert_equal(self.blackboard.get_node_status(self.CONTINUE1._id), 1154 | NodeStatus.CANCEL) 1155 | assert_equal(self.blackboard.get_node_status(self.CONTINUE2._id), 1156 | NodeStatus.CANCEL) 1157 | -------------------------------------------------------------------------------- /test/test_decorator.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | 16 | from nose.tools import assert_equal 17 | 18 | from task_behavior_engine.decorator import Fail 19 | from task_behavior_engine.decorator import Negate 20 | from task_behavior_engine.decorator import Repeat 21 | from task_behavior_engine.decorator import Succeed 22 | from task_behavior_engine.decorator import Until 23 | from task_behavior_engine.decorator import While 24 | 25 | from task_behavior_engine.node import Continue 26 | from task_behavior_engine.node import Fail as Failure 27 | from task_behavior_engine.node import Success 28 | 29 | from task_behavior_engine.tree import Blackboard 30 | from task_behavior_engine.tree import Node 31 | from task_behavior_engine.tree import NodeStatus 32 | 33 | 34 | class TestNegate(object): 35 | 36 | def setUp(self): 37 | self.blackboard = Blackboard() 38 | 39 | self.FAILURE = Failure(name="FAILURE", blackboard=self.blackboard) 40 | self.SUCCESS = Success(name="SUCCESS", blackboard=self.blackboard) 41 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 42 | 43 | def test_fail(self): 44 | NOT_FAIL = Negate(name="NOT_FAILURE", 45 | blackboard=self.blackboard, 46 | child=self.FAILURE) 47 | result = NOT_FAIL.tick() 48 | assert_equal(result, NodeStatus.SUCCESS) 49 | 50 | def test_success(self): 51 | NOT_SUCCESS = Negate(name="NOT_SUCCESS", 52 | blackboard=self.blackboard, 53 | child=self.SUCCESS) 54 | result = NOT_SUCCESS.tick() 55 | assert_equal(result, NodeStatus.FAIL) 56 | 57 | def test_active(self): 58 | ACTIVE = Negate(name="NOT_CONTINUE", 59 | blackboard=self.blackboard, 60 | child=self.CONTINUE) 61 | result = ACTIVE.tick() 62 | assert_equal(result, NodeStatus.ACTIVE) 63 | 64 | def test_cancel(self): 65 | ACTIVE = Negate(name="NOT_CONTINUE", 66 | blackboard=self.blackboard, 67 | child=self.CONTINUE) 68 | result = ACTIVE.tick() 69 | assert_equal(result, NodeStatus.ACTIVE) 70 | ACTIVE._cancel() 71 | assert_equal(ACTIVE.get_result(), NodeStatus.CANCEL) 72 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 73 | result = ACTIVE.tick() 74 | assert_equal(result, NodeStatus.CANCEL) 75 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 76 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 77 | 78 | def test_force(self): 79 | NOT_SUCCESS = Negate(name="NOT_SUCCESS", 80 | blackboard=self.blackboard, 81 | child=self.SUCCESS) 82 | NOT_SUCCESS._child.force(NodeStatus.FAIL) 83 | result = NOT_SUCCESS.tick() 84 | assert_equal(result, NodeStatus.SUCCESS) 85 | 86 | # Test FORCE behavior 87 | NOT_FAIL = Negate(name="NOT_FAILURE", 88 | blackboard=self.blackboard, 89 | child=self.FAILURE) 90 | NOT_FAIL.force(NodeStatus.FAIL) 91 | result = NOT_FAIL.tick() 92 | assert_equal(result, NodeStatus.FAIL) 93 | 94 | 95 | class TestRepeat(object): 96 | 97 | def setUp(self): 98 | self.blackboard = Blackboard() 99 | 100 | self.FAILURE = Failure(name="FAILURE", blackboard=self.blackboard) 101 | self.SUCCESS = Success(name="SUCCESS", blackboard=self.blackboard) 102 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 103 | 104 | def test_fail(self): 105 | REPEAT_FAIL = Repeat(name="REPEAT_FAILURE", 106 | blackboard=self.blackboard, 107 | child=self.FAILURE) 108 | result = REPEAT_FAIL.tick() 109 | assert_equal(result, NodeStatus.ACTIVE) 110 | assert_equal(self.blackboard.get_node_status(REPEAT_FAIL._id), 111 | NodeStatus.ACTIVE) 112 | assert_equal(self.blackboard.get_node_status(self.FAILURE._id), 113 | NodeStatus.FAIL) 114 | 115 | def test_success(self): 116 | REPEAT_SUCCESS = Repeat(name="REPEAT_SUCCESS", 117 | blackboard=self.blackboard, 118 | child=self.SUCCESS) 119 | result = REPEAT_SUCCESS.tick() 120 | assert_equal(result, NodeStatus.ACTIVE) 121 | assert_equal(self.blackboard.get_node_status(REPEAT_SUCCESS._id), 122 | NodeStatus.ACTIVE) 123 | assert_equal(self.blackboard.get_node_status(self.SUCCESS._id), 124 | NodeStatus.SUCCESS) 125 | 126 | def test_active(self): 127 | ACTIVE = Repeat(name="REPEAT_CONTINUE", 128 | blackboard=self.blackboard, 129 | child=self.CONTINUE) 130 | result = ACTIVE.tick() 131 | assert_equal(result, NodeStatus.ACTIVE) 132 | assert_equal(self.blackboard.get_node_status(ACTIVE._id), 133 | NodeStatus.ACTIVE) 134 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 135 | NodeStatus.ACTIVE) 136 | 137 | def test_cancel(self): 138 | ACTIVE = Repeat(name="REPEAT_CONTINUE", 139 | blackboard=self.blackboard, 140 | child=self.CONTINUE) 141 | result = ACTIVE.tick() 142 | ACTIVE._cancel() 143 | assert_equal(ACTIVE.get_result(), NodeStatus.CANCEL) 144 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 145 | assert_equal(self.blackboard.get_node_status(ACTIVE._id), 146 | NodeStatus.CANCEL) 147 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 148 | NodeStatus.CANCEL) 149 | result = ACTIVE.tick() 150 | assert_equal(result, NodeStatus.CANCEL) 151 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 152 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 153 | assert_equal(self.blackboard.get_node_status(ACTIVE._id), 154 | NodeStatus.CANCEL) 155 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 156 | NodeStatus.CANCEL) 157 | 158 | def test_cancel_child(self): 159 | ACTIVE = Repeat(name="REPEAT_CONTINUE", 160 | blackboard=self.blackboard, 161 | child=self.CONTINUE) 162 | ACTIVE.tick() 163 | ACTIVE._child._cancel() 164 | assert_equal(ACTIVE.get_result(), NodeStatus.ACTIVE) 165 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 166 | assert_equal(self.blackboard.get_node_status(ACTIVE._id), 167 | NodeStatus.ACTIVE) 168 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 169 | NodeStatus.CANCEL) 170 | result = ACTIVE.tick() 171 | assert_equal(result.status, NodeStatus.CANCEL) 172 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 173 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 174 | assert_equal(self.blackboard.get_node_status(ACTIVE._id), 175 | NodeStatus.CANCEL) 176 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 177 | NodeStatus.CANCEL) 178 | 179 | def test_force_child(self): 180 | NOT_SUCCESS = Repeat(name="NOT_SUCCESS", 181 | blackboard=self.blackboard, 182 | child=self.SUCCESS) 183 | NOT_SUCCESS._child.force(NodeStatus.FAIL) 184 | result = NOT_SUCCESS.tick() 185 | assert_equal(result, NodeStatus.ACTIVE) 186 | assert_equal(self.blackboard.get_node_status(NOT_SUCCESS._id), 187 | NodeStatus.ACTIVE) 188 | assert_equal(self.blackboard.get_node_status(self.SUCCESS._id), 189 | NodeStatus.FAIL) 190 | 191 | def test_force_behavior(self): 192 | NOT_FAIL = Repeat(name="NOT_FAILURE", 193 | blackboard=self.blackboard, 194 | child=self.FAILURE) 195 | result = NOT_FAIL.tick() 196 | NOT_FAIL.force(NodeStatus.SUCCESS) 197 | result = NOT_FAIL.tick() 198 | assert_equal(result, NodeStatus.SUCCESS) 199 | assert_equal(self.blackboard.get_node_status(NOT_FAIL._id), 200 | NodeStatus.SUCCESS) 201 | assert_equal(self.blackboard.get_node_status(self.FAILURE._id), 202 | NodeStatus.FAIL) 203 | 204 | NOT_CONTINUE = Repeat(name="NOT_CONTINUE", 205 | blackboard=self.blackboard, 206 | child=self.CONTINUE) 207 | result = NOT_CONTINUE.tick() 208 | NOT_CONTINUE.force(NodeStatus.FAIL) 209 | result = NOT_CONTINUE.tick() 210 | assert_equal(result, NodeStatus.FAIL) 211 | assert_equal(self.blackboard.get_node_status(NOT_CONTINUE._id), 212 | NodeStatus.FAIL) 213 | assert_equal(self.blackboard.get_node_status(self.CONTINUE._id), 214 | NodeStatus.CANCEL) 215 | 216 | 217 | class TestWhile(object): 218 | 219 | def setUp(self): 220 | self.blackboard = Blackboard() 221 | 222 | self.FAILURE = Failure(name="FAILURE", blackboard=self.blackboard) 223 | self.SUCCESS = Success(name="SUCCESS", blackboard=self.blackboard) 224 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 225 | 226 | def test_fail(self): 227 | WHILE_FAIL = While(name="WHILE_FAILURE", 228 | blackboard=self.blackboard, 229 | child=self.FAILURE) 230 | result = WHILE_FAIL.tick() 231 | assert_equal(result, NodeStatus.FAIL) 232 | 233 | def test_success(self): 234 | WHILE_SUCCESS = While(name="WHILE_SUCCESS", 235 | blackboard=self.blackboard, 236 | child=self.SUCCESS) 237 | result = WHILE_SUCCESS.tick() 238 | assert_equal(result, NodeStatus.ACTIVE) 239 | 240 | def test_active(self): 241 | ACTIVE = While(name="WHILE_CONTINUE", 242 | blackboard=self.blackboard, 243 | child=self.CONTINUE) 244 | result = ACTIVE.tick() 245 | assert_equal(result, NodeStatus.ACTIVE) 246 | 247 | def test_cancel(self): 248 | ACTIVE = While(name="WHILE_CONTINUE", 249 | blackboard=self.blackboard, 250 | child=self.CONTINUE) 251 | result = ACTIVE.tick() 252 | ACTIVE._cancel() 253 | assert_equal(ACTIVE.get_result(), NodeStatus.CANCEL) 254 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 255 | result = ACTIVE.tick() 256 | assert_equal(result, NodeStatus.CANCEL) 257 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 258 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 259 | 260 | def test_cancel_child(self): 261 | ACTIVE = While(name="WHILE_CONTINUE", 262 | blackboard=self.blackboard, 263 | child=self.CONTINUE) 264 | ACTIVE.tick() 265 | ACTIVE._child._cancel() 266 | assert_equal(ACTIVE.get_result(), NodeStatus.ACTIVE) 267 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 268 | result = ACTIVE.tick() 269 | assert_equal(result.status, NodeStatus.CANCEL) 270 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 271 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 272 | 273 | def test_force_child(self): 274 | NOT_SUCCESS = While(name="NOT_SUCCESS", 275 | blackboard=self.blackboard, 276 | child=self.SUCCESS) 277 | NOT_SUCCESS._child.force(NodeStatus.FAIL) 278 | result = NOT_SUCCESS.tick() 279 | assert_equal(result, NodeStatus.FAIL) 280 | 281 | def test_force_behavior(self): 282 | NOT_FAIL = While(name="NOT_FAILURE", 283 | blackboard=self.blackboard, 284 | child=self.FAILURE) 285 | NOT_FAIL.force(NodeStatus.SUCCESS) 286 | result = NOT_FAIL.tick() 287 | assert_equal(result, NodeStatus.SUCCESS) 288 | 289 | 290 | class TestUntil(object): 291 | 292 | def setUp(self): 293 | self.blackboard = Blackboard() 294 | 295 | self.FAILURE = Failure(name="FAILURE", blackboard=self.blackboard) 296 | self.SUCCESS = Success(name="SUCCESS", blackboard=self.blackboard) 297 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 298 | 299 | def test_fail(self): 300 | UNTIL_FAIL = Until(name="UNTIL_FAILURE", 301 | blackboard=self.blackboard, 302 | child=self.FAILURE) 303 | result = UNTIL_FAIL.tick() 304 | assert_equal(result, NodeStatus.ACTIVE) 305 | 306 | def test_success(self): 307 | UNTIL_SUCCESS = Until(name="UNTIL_SUCCESS", 308 | blackboard=self.blackboard, 309 | child=self.SUCCESS) 310 | result = UNTIL_SUCCESS.tick() 311 | assert_equal(result, NodeStatus.SUCCESS) 312 | 313 | def test_active(self): 314 | ACTIVE = Until(name="UNTIL_CONTINUE", 315 | blackboard=self.blackboard, 316 | child=self.CONTINUE) 317 | result = ACTIVE.tick() 318 | assert_equal(result, NodeStatus.ACTIVE) 319 | 320 | def test_cancel(self): 321 | ACTIVE = Until(name="UNTIL_CONTINUE", 322 | blackboard=self.blackboard, 323 | child=self.CONTINUE) 324 | result = ACTIVE.tick() 325 | ACTIVE._cancel() 326 | assert_equal(ACTIVE.get_result(), NodeStatus.CANCEL) 327 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 328 | result = ACTIVE.tick() 329 | assert_equal(result, NodeStatus.CANCEL) 330 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 331 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 332 | 333 | def test_cancel_child(self): 334 | ACTIVE = Until(name="UNTIL_CONTINUE", 335 | blackboard=self.blackboard, 336 | child=self.CONTINUE) 337 | ACTIVE.tick() 338 | ACTIVE._child._cancel() 339 | assert_equal(ACTIVE.get_result(), NodeStatus.ACTIVE) 340 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 341 | result = ACTIVE.tick() 342 | assert_equal(result.status, NodeStatus.CANCEL) 343 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 344 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 345 | 346 | def test_force_child(self): 347 | NOT_SUCCESS = Until(name="NOT_SUCCESS", 348 | blackboard=self.blackboard, 349 | child=self.SUCCESS) 350 | NOT_SUCCESS._child.force(NodeStatus.FAIL) 351 | result = NOT_SUCCESS.tick() 352 | assert_equal(result, NodeStatus.ACTIVE) 353 | 354 | def test_force_behavior(self): 355 | NOT_FAIL = Until(name="NOT_FAIL", 356 | blackboard=self.blackboard, 357 | child=self.FAILURE) 358 | NOT_FAIL.force(NodeStatus.SUCCESS) 359 | result = NOT_FAIL.tick() 360 | assert_equal(result, NodeStatus.SUCCESS) 361 | 362 | 363 | class TestFail(object): 364 | 365 | def setUp(self): 366 | self.blackboard = Blackboard() 367 | 368 | self.FAILURE = Failure(name="FAILURE", blackboard=self.blackboard) 369 | self.SUCCESS = Success(name="SUCCESS", blackboard=self.blackboard) 370 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 371 | 372 | def test_fail(self): 373 | TEST = Fail(name="FAIL_FAILURE", 374 | blackboard=self.blackboard, 375 | child=self.FAILURE) 376 | result = TEST.tick() 377 | assert_equal(result, NodeStatus.FAIL) 378 | 379 | def test_success(self): 380 | TEST = Fail(name="FAIL_SUCCESS", 381 | blackboard=self.blackboard, 382 | child=self.SUCCESS) 383 | result = TEST.tick() 384 | assert_equal(result, NodeStatus.FAIL) 385 | 386 | def test_active(self): 387 | ACTIVE = Fail(name="FAIL_CONTINUE", 388 | blackboard=self.blackboard, 389 | child=self.CONTINUE) 390 | result = ACTIVE.tick() 391 | assert_equal(result, NodeStatus.ACTIVE) 392 | 393 | def test_cancel(self): 394 | ACTIVE = Fail(name="FAIL_CONTINUE", 395 | blackboard=self.blackboard, 396 | child=self.CONTINUE) 397 | result = ACTIVE.tick() 398 | ACTIVE._cancel() 399 | assert_equal(ACTIVE.get_result(), NodeStatus.CANCEL) 400 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 401 | result = ACTIVE.tick() 402 | assert_equal(result, NodeStatus.CANCEL) 403 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 404 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 405 | 406 | def test_cancel_child(self): 407 | ACTIVE = Fail(name="FAIL_CONTINUE", 408 | blackboard=self.blackboard, 409 | child=self.CONTINUE) 410 | ACTIVE.tick() 411 | ACTIVE._child._cancel() 412 | assert_equal(ACTIVE.get_result(), NodeStatus.ACTIVE) 413 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 414 | result = ACTIVE.tick() 415 | assert_equal(result.status, NodeStatus.CANCEL) 416 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 417 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 418 | 419 | def test_force_child(self): 420 | TEST = Fail(name="SUCCESS_CHILD", 421 | blackboard=self.blackboard, 422 | child=self.CONTINUE) 423 | TEST._child.force(NodeStatus.SUCCESS) 424 | result = TEST.tick() 425 | assert_equal(result, NodeStatus.FAIL) 426 | 427 | def test_force_behavior(self): 428 | TEST = Fail(name="FORCE_SUCCESS", 429 | blackboard=self.blackboard, 430 | child=self.CONTINUE) 431 | TEST.force(NodeStatus.SUCCESS) 432 | result = TEST.tick() 433 | assert_equal(result, NodeStatus.SUCCESS) 434 | 435 | 436 | class TestSucceed(object): 437 | 438 | def setUp(self): 439 | self.blackboard = Blackboard() 440 | 441 | self.FAILURE = Failure(name="FAILURE", blackboard=self.blackboard) 442 | self.SUCCESS = Success(name="SUCCESS", blackboard=self.blackboard) 443 | self.CONTINUE = Continue(name="CONTINUE", blackboard=self.blackboard) 444 | 445 | def test_fail(self): 446 | TEST = Succeed(name="SUCCEED_FAILURE", 447 | blackboard=self.blackboard, 448 | child=self.FAILURE) 449 | result = TEST.tick() 450 | assert_equal(result, NodeStatus.SUCCESS) 451 | 452 | def test_success(self): 453 | TEST = Succeed(name="SUCCEED_SUCCESS", 454 | blackboard=self.blackboard, 455 | child=self.SUCCESS) 456 | result = TEST.tick() 457 | assert_equal(result, NodeStatus.SUCCESS) 458 | 459 | def test_active(self): 460 | ACTIVE = Succeed(name="SUCCEED_CONTINUE", 461 | blackboard=self.blackboard, 462 | child=self.CONTINUE) 463 | result = ACTIVE.tick() 464 | assert_equal(result, NodeStatus.ACTIVE) 465 | 466 | def test_cancel(self): 467 | ACTIVE = Succeed(name="SUCCEED_CONTINUE", 468 | blackboard=self.blackboard, 469 | child=self.CONTINUE) 470 | result = ACTIVE.tick() 471 | ACTIVE._cancel() 472 | assert_equal(ACTIVE.get_result(), NodeStatus.CANCEL) 473 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 474 | result = ACTIVE.tick() 475 | assert_equal(result, NodeStatus.CANCEL) 476 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 477 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 478 | 479 | def test_cancel_child(self): 480 | ACTIVE = Succeed(name="SUCCEED_CONTINUE", 481 | blackboard=self.blackboard, 482 | child=self.CONTINUE) 483 | ACTIVE.tick() 484 | ACTIVE._child._cancel() 485 | assert_equal(ACTIVE.get_result(), NodeStatus.ACTIVE) 486 | assert_equal(ACTIVE._child.get_result(), NodeStatus.CANCEL) 487 | result = ACTIVE.tick() 488 | assert_equal(result.status, NodeStatus.CANCEL) 489 | assert_equal(ACTIVE.get_result(), NodeStatus.PENDING) 490 | assert_equal(ACTIVE._child.get_result(), NodeStatus.PENDING) 491 | 492 | def test_force_child(self): 493 | TEST = Succeed(name="SUCCESS_CHILD", 494 | blackboard=self.blackboard, 495 | child=self.CONTINUE) 496 | TEST._child.force(NodeStatus.FAIL) 497 | result = TEST.tick() 498 | assert_equal(result, NodeStatus.SUCCESS) 499 | 500 | def test_force_behavior(self): 501 | TEST = Succeed(name="FORCE_SUCCESS", 502 | blackboard=self.blackboard, 503 | child=self.CONTINUE) 504 | TEST.force(NodeStatus.FAIL) 505 | result = TEST.tick() 506 | assert_equal(result, NodeStatus.FAIL) 507 | -------------------------------------------------------------------------------- /test/test_tree.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Toyota Research Institute 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); you may not 4 | # use this file except in compliance with the License. You may obtain a copy 5 | # of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 11 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 12 | # License for the specific language governing permissions and limitations 13 | # under the License. 14 | 15 | from nose.tools import assert_equal 16 | from nose.tools import assert_not_equal 17 | from nose.tools import assert_raises 18 | 19 | from task_behavior_engine.tree import Behavior 20 | from task_behavior_engine.tree import Blackboard 21 | from task_behavior_engine.tree import Decorator 22 | from task_behavior_engine.tree import Node 23 | from task_behavior_engine.tree import NodeData 24 | from task_behavior_engine.tree import NodeStatus 25 | 26 | 27 | class TestNodeData(object): 28 | 29 | def test_init(self): 30 | nd = NodeData() 31 | assert_equal(nd._data, {}) 32 | assert_equal(nd._locks, {}) 33 | 34 | def test_assignment(self): 35 | nd = NodeData() 36 | # check variable assignment 37 | nd.test = "hello world" 38 | assert_equal(nd._data['test'], 'hello world') 39 | # check dictionary assignment 40 | nd['foo'] = "bar" 41 | assert_equal(nd._data['foo'], 'bar') 42 | # check set_data 43 | nd.set_data("universe", 47) 44 | assert_equal(nd._data['universe'], 47) 45 | 46 | def test_keys(self): 47 | nd = NodeData() 48 | nd.key1 = "first key" 49 | assert_equal(nd.keys(), ['key1']) 50 | nd.key2 = "second key" 51 | assert_equal(True, 'key2' in nd.keys()) 52 | assert_equal(True, 'key1' in nd.keys()) 53 | 54 | def test_get_data(self): 55 | nd = NodeData() 56 | # check retrieval from set 57 | nd.set_data("universe", 47) 58 | assert_equal(nd.get_data("universe"), 47) 59 | # check default operator 60 | assert_equal(nd.get_data("ros", "bag"), "bag") 61 | # check retrieval of non-existing data 62 | assert_equal(nd.get_data("none"), None) 63 | 64 | def test_contains(self): 65 | nd = NodeData() 66 | nd.test = "hello world" 67 | assert_equal(("test" in nd), True) 68 | assert_not_equal(("foo" in nd), True) 69 | 70 | 71 | class TestNodeStatus(object): 72 | 73 | def test_init(self): 74 | s0 = NodeStatus() 75 | assert_equal(s0.status, NodeStatus.PENDING) 76 | assert_equal(s0.text, '') 77 | 78 | s1 = NodeStatus(NodeStatus.ACTIVE, 'testing init') 79 | assert_equal(s1.status, NodeStatus.ACTIVE) 80 | assert_equal(s1.text, 'testing init') 81 | 82 | s2 = NodeStatus(NodeStatus.SUCCESS) 83 | assert_equal(s2.status, NodeStatus.SUCCESS) 84 | assert_equal(s2.text, '') 85 | 86 | def test_str(self): 87 | s0 = NodeStatus(NodeStatus.PENDING, 'not started') 88 | assert_equal(str(s0), 'PENDING not started') 89 | 90 | s1 = NodeStatus(NodeStatus.ACTIVE, 'running') 91 | assert_equal(str(s1), 'ACTIVE running') 92 | 93 | s2 = NodeStatus(NodeStatus.SUCCESS, 'finished') 94 | assert_equal(str(s2), 'SUCCESS finished') 95 | 96 | s3 = NodeStatus(NodeStatus.FAIL, 'finished') 97 | assert_equal(str(s3), 'FAIL finished') 98 | 99 | s4 = NodeStatus(NodeStatus.CANCEL, 'canceled') 100 | assert_equal(str(s4), 'CANCEL canceled') 101 | 102 | s5 = NodeStatus(5, 'unknown') 103 | assert_equal(str(s5), '5 unknown') 104 | 105 | def test_eq(self): 106 | assert_equal(NodeStatus(NodeStatus.PENDING), NodeStatus.PENDING) 107 | assert_equal(NodeStatus(NodeStatus.ACTIVE), NodeStatus.ACTIVE) 108 | assert_equal(NodeStatus(NodeStatus.SUCCESS), NodeStatus.SUCCESS) 109 | assert_equal(NodeStatus(NodeStatus.FAIL), NodeStatus.FAIL) 110 | assert_equal(NodeStatus(NodeStatus.CANCEL), NodeStatus.CANCEL) 111 | 112 | assert_not_equal(NodeStatus(), NodeStatus(NodeStatus.ACTIVE)) 113 | assert_not_equal(NodeStatus(NodeStatus.FAIL), NodeStatus.SUCCESS) 114 | 115 | 116 | class TestBlackboard(object): 117 | 118 | def test_init(self): 119 | b = Blackboard() 120 | assert_equal(b._base_memory, {}) 121 | assert_equal(b._node_memory, {}) 122 | 123 | def test_base_memory(self): 124 | b = Blackboard() 125 | # test manually set memory 126 | b._base_memory["test"] = "hello world" 127 | assert_equal(b._get_memory(), {'test': 'hello world'}) 128 | # test save 129 | b.save("foo", "bar") 130 | assert_equal(b._base_memory['foo'], 'bar') 131 | # test get 132 | assert_equal(b.get("foo"), 'bar') 133 | 134 | def test_node_memory(self): 135 | b = Blackboard() 136 | # test manually set memory 137 | b._node_memory["uuid"] = {} 138 | assert_equal(b._get_node_memory("uuid"), {}) 139 | # test unknown uuid memory 140 | memory = b._get_node_memory("test_uuid") 141 | assert_equal(memory.keys(), ['node_data', 'remapping']) 142 | # test _get_node_memory 143 | memory['test_memory'] = 'test' 144 | memory = b._get_node_memory("test_uuid") 145 | assert_equal(memory['test_memory'], 'test') 146 | 147 | def test_memory(self): 148 | b = Blackboard() 149 | # test manually set memory 150 | node_memory = {'node_data': {'test_memory': 'test'}, 'remapping': {}} 151 | node_data = b._get_node_data(node_memory) 152 | assert_equal(node_data.keys(), ['test_memory']) 153 | # test non-existing memory 154 | assert_raises(KeyError, b.get, 'test', 'node_scope') 155 | # test save 156 | b.save('foo', 'bar', 'node_scope') 157 | value = b.get('foo', 'node_scope') 158 | assert_equal(value, 'bar') 159 | # test setting nodedata 160 | nd = b.get_memory('node_scope') 161 | nd.hello = "world" 162 | assert_equal(b.get('hello', 'node_scope'), 'world') 163 | 164 | def test_remapping(self): 165 | b = Blackboard() 166 | b.save('foo', 'bar', 'scope1') 167 | b.save('hello', 'world', 'scope2') 168 | b.add_remapping('scope1', 'foo', 'scope2', 'new_foo') 169 | scope1_nodedata = b.get_memory('scope2') 170 | assert_equal(scope1_nodedata['hello'], 'world') 171 | assert_equal(scope1_nodedata['new_foo'], 'bar') 172 | assert_equal(b.get('hello', 'scope2'), 'world') 173 | assert_equal(b.get('new_foo', 'scope2'), 'bar') 174 | # Test saving a value onto a remap gets over-written 175 | b.save('new_foo', 'test', 'scope2') 176 | assert_equal(b.get('foo', 'scope1'), 'bar') 177 | assert_equal(b.get('new_foo', 'scope2'), 'bar') 178 | # Test local changes get updated 179 | scope1_nodedata = b.get_memory('scope2') 180 | scope1_nodedata.new_foo = 'test' 181 | assert_equal(scope1_nodedata.new_foo, 'test') 182 | # Test updating a source remap, updates destination remap 183 | b.save('foo', 'new', 'scope1') 184 | assert_equal(b.get('foo', 'scope1'), 'new') 185 | assert_equal(b.get('new_foo', 'scope2'), 'new') 186 | # Test adding remapping to same value fails 187 | b.save('ping', 'pong', 'scope1') 188 | assert_raises(RuntimeError, 189 | b.add_remapping, 'scope1', 'ping', 'scope2', 'new_foo') 190 | 191 | def test_node_status(self): 192 | b = Blackboard() 193 | assert_equal(b.get_node_status("blah"), NodeStatus.PENDING) 194 | 195 | b.set_node_status("scope1", NodeStatus.ACTIVE) 196 | assert_equal(b.get_node_status("scope1"), NodeStatus.ACTIVE) 197 | 198 | status = b.get_status() 199 | assert_equal(status["blah"], NodeStatus.PENDING) 200 | assert_equal(status['scope1'], NodeStatus.ACTIVE) 201 | 202 | b.clear_node_status() 203 | status = b.get_status() 204 | assert_equal(status, {}) 205 | 206 | 207 | class TestNode(object): 208 | 209 | def test_init(self): 210 | assert_raises(Exception, Node) 211 | n = Node('test') 212 | assert_equal(n._name, 'test') 213 | assert_equal(n._result, NodeStatus.PENDING) 214 | 215 | def test_blackboard(self): 216 | blackboard = Blackboard() 217 | n = Node('test', blackboard) 218 | assert_equal(n._blackboard, blackboard) 219 | 220 | n = Node('test') 221 | assert_not_equal(n._blackboard, blackboard) 222 | n.set_blackboard(blackboard) 223 | assert_equal(n._blackboard, blackboard) 224 | 225 | def test_configure(self): 226 | # test normal configure cb 227 | def configure_callback(nd): 228 | nd.cb_called = True 229 | n = Node('test', configure_cb=configure_callback) 230 | nd = n._blackboard.get_memory(n._id) 231 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 232 | n._configure() 233 | assert_equal(nd.cb_called, True) 234 | assert_equal(n._result, NodeStatus.ACTIVE) 235 | # test no configure cb 236 | n = Node('test') 237 | nd = n._blackboard.get_memory(n._id) 238 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 239 | n._configure() 240 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 241 | assert_equal(n._result, NodeStatus.ACTIVE) 242 | # test register configure cb 243 | n = Node('test') 244 | n.register_configure_cb(configure_callback) 245 | nd = n._blackboard.get_memory(n._id) 246 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 247 | n._configure() 248 | assert_equal(nd.cb_called, True) 249 | assert_equal(n._result, NodeStatus.ACTIVE) 250 | 251 | def test_cleanup(self): 252 | # test normal cleanup 253 | def cleanup_callback(nd): 254 | nd.cb_called = True 255 | n = Node('test', cleanup_cb=cleanup_callback) 256 | nd = n._blackboard.get_memory(n._id) 257 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 258 | n._cleanup() 259 | assert_equal(nd.cb_called, True) 260 | assert_equal(n._result, NodeStatus.PENDING) 261 | # test no exit 262 | n = Node('test') 263 | nd = n._blackboard.get_memory(n._id) 264 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 265 | n._cleanup() 266 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 267 | assert_equal(n._result, NodeStatus.PENDING) 268 | # test register 269 | n = Node('test') 270 | nd = n._blackboard.get_memory(n._id) 271 | n.register_cleanup_cb(cleanup_callback) 272 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 273 | n._cleanup() 274 | assert_equal(nd.cb_called, True) 275 | assert_equal(n._result, NodeStatus.PENDING) 276 | 277 | def test_run(self): 278 | # test normal evaluate 279 | def run_callback(nd): 280 | nd.cb_called = True 281 | return NodeStatus(NodeStatus.SUCCESS, 'complete') 282 | n = Node('test', run_cb=run_callback) 283 | nd = n._blackboard.get_memory(n._id) 284 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 285 | r = n._run() 286 | assert_equal(nd.cb_called, True) 287 | assert_equal(r, NodeStatus.SUCCESS) 288 | # test no evaluate 289 | n = Node('test') 290 | nd = n._blackboard.get_memory(n._id) 291 | nd.cb_called = False 292 | assert_raises(NotImplementedError, n._run) 293 | # test register 294 | n = Node('test') 295 | nd = n._blackboard.get_memory(n._id) 296 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 297 | n.register_run_cb(run_callback) 298 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 299 | n._run() 300 | assert_equal(nd.cb_called, True) 301 | 302 | def wrong_callback(nd): 303 | nd.cb_called = True 304 | return "" 305 | 306 | n = Node('test') 307 | nd = n._blackboard.get_memory(n._id) 308 | n.register_run_cb(wrong_callback) 309 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 310 | assert_raises(Exception, n._run) 311 | assert_equal(nd.cb_called, True) 312 | 313 | def test_cancel(self): 314 | # test normal cancel 315 | def cancel_callback(nd): 316 | nd.cb_called = True 317 | n = Node('test', cancel_cb=cancel_callback) 318 | nd = n._blackboard.get_memory(n._id) 319 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 320 | n._cancel() 321 | assert_equal(nd.cb_called, True) 322 | # test no cancel callback 323 | n = Node('test') 324 | nd = n._blackboard.get_memory(n._id) 325 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 326 | n._cancel() 327 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 328 | # test register 329 | n = Node('test') 330 | nd = n._blackboard.get_memory(n._id) 331 | n.register_cancel_cb(cancel_callback) 332 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 333 | n._cancel() 334 | assert_equal(nd.cb_called, True) 335 | 336 | def test_force(self): 337 | # test force 338 | def run_callback(nd): 339 | nd.cb_called = True 340 | return NodeStatus(NodeStatus.SUCCESS, 'complete') 341 | n = Node('test', run_cb=run_callback) 342 | nd = n._blackboard.get_memory(n._id) 343 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 344 | r = n._run() 345 | assert_equal(r, NodeStatus.SUCCESS) 346 | assert_equal(nd.cb_called, True) 347 | n = Node('test', run_cb=run_callback) 348 | nd = n._blackboard.get_memory(n._id) 349 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 350 | n.force(NodeStatus.FAIL) 351 | r = n._run() 352 | assert_equal(r, NodeStatus.FAIL) 353 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 354 | n._cleanup() 355 | n._configure() 356 | r = n._run() 357 | assert_equal(r, NodeStatus.SUCCESS) 358 | assert_equal(nd.cb_called, True) 359 | 360 | def test_result(self): 361 | 362 | def run_callback(nd): 363 | nd.cb_called = True 364 | return NodeStatus(NodeStatus.SUCCESS, 'complete') 365 | n = Node('test', run_cb=run_callback) 366 | nd = n._blackboard.get_memory(n._id) 367 | assert_equal(n.get_result(), NodeStatus.PENDING) 368 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 369 | n._configure() 370 | assert_equal(n.get_result(), NodeStatus.ACTIVE) 371 | assert_raises(KeyError, nd.__getitem__, 'cb_called') 372 | n._run() 373 | assert_equal(n.get_result(), NodeStatus.SUCCESS) 374 | assert_equal(nd.cb_called, True) 375 | n._cleanup() 376 | assert_equal(n.get_result(), NodeStatus.PENDING) 377 | assert_equal(nd.cb_called, True) 378 | 379 | def test_tick(self): 380 | def config_callback(nd): 381 | nd.config_called += 1 382 | 383 | def run_callback(nd): 384 | nd.run_called += 1 385 | return NodeStatus(NodeStatus.ACTIVE, 'running') 386 | 387 | def cancel_callback(nd): 388 | nd.cancel_called += 1 389 | 390 | n = Node('test', run_cb=run_callback, 391 | configure_cb=config_callback, cancel_cb=cancel_callback) 392 | nd = n._blackboard.get_memory(n._id) 393 | nd.config_called = 0 394 | nd.run_called = 0 395 | nd.cancel_called = 0 396 | 397 | assert_equal(n.get_result().status, NodeStatus.PENDING) 398 | assert_equal(nd.config_called, 0) 399 | assert_equal(nd.run_called, 0) 400 | assert_equal(nd.cancel_called, 0) 401 | 402 | result = n.tick() 403 | assert_equal(n.get_result().status, NodeStatus.ACTIVE) 404 | assert_equal(result.status, NodeStatus.ACTIVE) 405 | assert_equal(nd.config_called, 1) 406 | assert_equal(nd.run_called, 1) 407 | assert_equal(nd.cancel_called, 0) 408 | 409 | result = n.tick() 410 | assert_equal(n.get_result().status, NodeStatus.ACTIVE) 411 | assert_equal(result.status, NodeStatus.ACTIVE) 412 | assert_equal(nd.config_called, 1) 413 | assert_equal(nd.run_called, 2) 414 | assert_equal(nd.cancel_called, 0) 415 | 416 | n.cancel() 417 | assert_equal(n.get_result().status, NodeStatus.CANCEL) 418 | assert_equal(nd.config_called, 1) 419 | assert_equal(nd.run_called, 2) 420 | assert_equal(nd.cancel_called, 1) 421 | 422 | result = n.tick() 423 | assert_equal(n.get_result().status, NodeStatus.PENDING) 424 | assert_equal(result.status, NodeStatus.CANCEL) 425 | assert_equal(nd.config_called, 1) 426 | assert_equal(nd.run_called, 2) 427 | assert_equal(nd.cancel_called, 1) 428 | 429 | 430 | class TestDecorator(object): 431 | 432 | def test_init(self): 433 | def run_callback(nd): 434 | nd.cb_called = True 435 | return NodeStatus(NodeStatus.SUCCESS, 'complete') 436 | n = Node('test', run_cb=run_callback) 437 | d = Decorator('d_test', n) 438 | assert_equal(d._child, n) 439 | 440 | def test_configure(self): 441 | n1 = Node('node1') 442 | 443 | def success_cb(nd): 444 | nd.called += "run " 445 | return NodeStatus(NodeStatus.SUCCESS) 446 | 447 | def config_cb(nd): 448 | nd.called = nd.get_data("called", "") 449 | nd.called += "config " 450 | 451 | def cleanup_cb(nd): 452 | nd.called += "cleanup " 453 | 454 | n1.register_configure_cb(config_cb) 455 | n1.register_run_cb(success_cb) 456 | n1.register_cleanup_cb(cleanup_cb) 457 | 458 | d = Decorator('d_test', n1) 459 | d._configure() 460 | assert_equal(d.get_result(), NodeStatus.ACTIVE) 461 | assert_equal(d._child.get_result(), NodeStatus.ACTIVE) 462 | nd = n1._blackboard.get_memory(n1._id) 463 | assert_equal(nd.called, "config ") 464 | 465 | def test_cleanup(self): 466 | n1 = Node('node1') 467 | 468 | def success_cb(nd): 469 | nd.called += "run " 470 | return NodeStatus(NodeStatus.SUCCESS) 471 | 472 | def config_cb(nd): 473 | nd.called = nd.get_data("called", "") 474 | nd.called += "config " 475 | 476 | def cleanup_cb(nd): 477 | nd.called = nd.get_data("called", "") 478 | nd.called += "cleanup " 479 | 480 | n1.register_configure_cb(config_cb) 481 | n1.register_run_cb(success_cb) 482 | n1.register_cleanup_cb(cleanup_cb) 483 | 484 | d = Decorator('d_test', n1) 485 | d._cleanup() 486 | assert_equal(d.get_result(), NodeStatus.PENDING) 487 | assert_equal(d._child.get_result(), NodeStatus.PENDING) 488 | nd = n1._blackboard.get_memory(n1._id) 489 | assert_equal(nd.called, "cleanup ") 490 | 491 | def test_cancel(self): 492 | n1 = Node('node1') 493 | 494 | def success_cb(nd): 495 | nd.called += "run " 496 | return NodeStatus(NodeStatus.SUCCESS) 497 | 498 | def config_cb(nd): 499 | nd.called = nd.get_data("called", "") 500 | nd.called += "config " 501 | 502 | def cleanup_cb(nd): 503 | nd.called += "cleanup " 504 | 505 | n1.register_configure_cb(config_cb) 506 | n1.register_run_cb(success_cb) 507 | n1.register_cleanup_cb(cleanup_cb) 508 | 509 | d = Decorator('d_test', n1) 510 | d._cancel() 511 | assert_equal(d.get_result(), NodeStatus.CANCEL) 512 | assert_equal(d._child.get_result(), NodeStatus.CANCEL) 513 | 514 | 515 | class TestBehavior(object): 516 | 517 | def test_init(self): 518 | b = Behavior('test') 519 | assert_equal(b._children, []) 520 | 521 | def test_children(self): 522 | b = Behavior('test') 523 | n1 = Node('node1') 524 | n2 = Node('node2') 525 | # Test adding children 526 | b.add_child(n1) 527 | assert_equal(b._children, [n1]) 528 | b.add_child(n2) 529 | assert_equal(b._children, [n1, n2]) 530 | # Test removing children 531 | b.remove_child(n2) 532 | assert_equal(b._children, [n1]) 533 | # Test prepending child 534 | b.prepend_child(n2) 535 | assert_equal(b._children, [n2, n1]) 536 | # Test inserting child 537 | n3 = Node('node3') 538 | b.insert_child(n3, 1) 539 | assert_equal(b._children, [n2, n3, n1]) 540 | # Test insertring child with same name raises 541 | n4 = Node('node1') 542 | assert_raises(RuntimeError, 543 | b.add_child, n4) 544 | 545 | def test_blackboard(self): 546 | blackboard = Blackboard() 547 | b = Behavior(name='test', blackboard=blackboard) 548 | assert_equal(b._blackboard, blackboard) 549 | 550 | b = Behavior(name='test') 551 | assert_not_equal(b._blackboard, blackboard) 552 | b.set_blackboard(blackboard) 553 | assert_equal(b._blackboard, blackboard) 554 | 555 | def test_tick_child(self): 556 | blackboard = Blackboard() 557 | n1 = Node(name='node1', blackboard=blackboard) 558 | 559 | def success_cb(nd): 560 | nd.called += "run " 561 | return NodeStatus(NodeStatus.SUCCESS) 562 | 563 | def config_cb(nd): 564 | nd.called = nd.get_data("called", "") 565 | nd.called += "config " 566 | 567 | def cleanup_cb(nd): 568 | nd.called += "cleanup " 569 | 570 | n1.register_configure_cb(config_cb) 571 | n1.register_run_cb(success_cb) 572 | n1.register_cleanup_cb(cleanup_cb) 573 | 574 | b = Behavior(name='test', blackboard=blackboard) 575 | b.add_child(n1) 576 | r = b.tick_child(n1) 577 | assert_equal(r, NodeStatus.SUCCESS) 578 | steps = b._blackboard.get('called', n1._id) 579 | assert_equal(steps, "config run cleanup ") 580 | 581 | def test_cancel(self): 582 | blackboard = Blackboard() 583 | n1 = Node(name='node1', blackboard=blackboard) 584 | 585 | def continue_cb(nd): 586 | nd.called += "run " 587 | return NodeStatus(NodeStatus.ACTIVE) 588 | 589 | def config_cb(nd): 590 | nd.called = nd.get_data("called", "") 591 | nd.called += "config " 592 | 593 | def cleanup_cb(nd): 594 | nd.called += "cleanup " 595 | 596 | def cancel_cb(nd): 597 | nd.called += "cancel " 598 | 599 | n1.register_configure_cb(config_cb) 600 | n1.register_run_cb(continue_cb) 601 | n1.register_cleanup_cb(cleanup_cb) 602 | n1.register_cancel_cb(cancel_cb) 603 | 604 | b = Behavior(name='test', blackboard=blackboard) 605 | b.add_child(n1) 606 | r = b.tick_child(n1) 607 | assert_equal(r, NodeStatus.ACTIVE) 608 | b._cancel() 609 | r = b.tick_child(n1) 610 | assert_equal(r, NodeStatus.CANCEL) 611 | steps = b._blackboard.get('called', n1._id) 612 | assert_equal(steps, "config run cancel cleanup ") 613 | 614 | def test_cleanup(self): 615 | blackboard = Blackboard() 616 | n1 = Node(name='node1', blackboard=blackboard) 617 | n2 = Node(name='node2', blackboard=blackboard) 618 | 619 | def success_cb(nd): 620 | nd.called += "success " 621 | return NodeStatus(NodeStatus.SUCCESS) 622 | 623 | def continue_cb(nd): 624 | nd.called += "run " 625 | return NodeStatus(NodeStatus.ACTIVE) 626 | 627 | def config_cb(nd): 628 | nd.called = nd.get_data("called", "") 629 | nd.called += "config " 630 | 631 | def cleanup_cb(nd): 632 | nd.called += "cleanup " 633 | 634 | def cancel_cb(nd): 635 | nd.called += "cancel " 636 | 637 | n1.register_configure_cb(config_cb) 638 | n1.register_run_cb(continue_cb) 639 | n1.register_cleanup_cb(cleanup_cb) 640 | n1.register_cancel_cb(cancel_cb) 641 | 642 | n2.register_configure_cb(config_cb) 643 | n2.register_run_cb(success_cb) 644 | n2.register_cleanup_cb(cleanup_cb) 645 | n2.register_cancel_cb(cancel_cb) 646 | 647 | b = Behavior(name='test', blackboard=blackboard) 648 | b.add_child(n1) 649 | b.add_child(n2) 650 | 651 | b._configure() 652 | r = b.tick_child(n1) 653 | assert_equal(r, NodeStatus.ACTIVE) 654 | r = b.tick_child(n2) 655 | assert_equal(r, NodeStatus.SUCCESS) 656 | 657 | assert_equal(n1.get_result().status, NodeStatus.ACTIVE) 658 | assert_equal(n2.get_result().status, NodeStatus.PENDING) 659 | assert_equal(b.get_result().status, NodeStatus.ACTIVE) 660 | 661 | b._cleanup() 662 | 663 | assert_equal(n1.get_result().status, NodeStatus.PENDING) 664 | assert_equal(n2.get_result().status, NodeStatus.PENDING) 665 | assert_equal(b.get_result().status, NodeStatus.PENDING) 666 | --------------------------------------------------------------------------------