├── contributors.txt ├── behavior_tree_core ├── contributors.txt ├── action │ └── BT.action ├── package.xml ├── src │ ├── exceptions.cpp │ ├── leaf_node.cpp │ ├── condition_node.cpp │ ├── action_node.cpp │ ├── tick_engine.cpp │ ├── gtest │ │ ├── external_ros_nodes_test.cpp │ │ └── gtest_tree.cpp │ ├── conditions │ │ ├── ros_condition.cpp │ │ └── condition_test_node.cpp │ ├── behavior_tree.cpp │ ├── tree.cpp │ ├── actions │ │ ├── action_test_node.cpp │ │ └── ros_action.cpp │ ├── tree_node.cpp │ ├── decorators │ │ └── negation_node.cpp │ ├── decorator_node.cpp │ ├── control_node.cpp │ ├── sequence_node.cpp │ ├── fallback_node.cpp │ ├── parallel_node.cpp │ ├── fallback_node_with_memory.cpp │ ├── sequence_node_with_memory.cpp │ ├── dot_bt.cpp │ └── draw.cpp ├── include │ ├── leaf_node.h │ ├── tick_engine.h │ ├── exceptions.h │ ├── sequence_node.h │ ├── decorators │ │ └── negation_node.h │ ├── fallback_node.h │ ├── decorator_node.h │ ├── conditions │ │ ├── condition_test_node.h │ │ └── ros_condition.h │ ├── actions │ │ ├── action_test_node.h │ │ └── ros_action.h │ ├── fallback_node_with_memory.h │ ├── parallel_node.h │ ├── sequence_node_with_memory.h │ ├── condition_node.h │ ├── action_node.h │ ├── behavior_tree.h │ ├── draw.h │ ├── control_node.h │ ├── tree_node.h │ └── dot_bt.h ├── templates │ ├── condition_node_template.cpp │ └── action_node_template.cpp └── CMakeLists.txt ├── behavior_tree_leaves ├── contributors.txt ├── launch │ └── test_behavior_tree.launch ├── example_nodes │ ├── python │ │ ├── class_example.py │ │ ├── bt_action.py │ │ ├── condition_example.py │ │ └── action_example.py │ └── cpp │ │ ├── class_example.cpp │ │ ├── bt_action.h │ │ ├── condition_example.cpp │ │ └── action_example.cpp ├── package.xml ├── src │ ├── action_client.cpp │ └── condition_client.cpp └── CMakeLists.txt ├── BTUserManual.pdf ├── behavior_tree ├── CMakeLists.txt └── package.xml ├── LICENSE └── README.md /contributors.txt: -------------------------------------------------------------------------------- 1 | Michele Colledanchise 2 | -------------------------------------------------------------------------------- /behavior_tree_core/contributors.txt: -------------------------------------------------------------------------------- 1 | Michele Colledanchise 2 | -------------------------------------------------------------------------------- /behavior_tree_leaves/contributors.txt: -------------------------------------------------------------------------------- 1 | Michele Colledanchise 2 | -------------------------------------------------------------------------------- /BTUserManual.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miccol/ROS-Behavior-Tree/HEAD/BTUserManual.pdf -------------------------------------------------------------------------------- /behavior_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(behavior_tree) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /behavior_tree_core/action/BT.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | int32 parameter #no goal needed for BT tick. But It could be useful to have a parameter for action (e.g.goto x,y) 3 | --- 4 | #result definition RUNNING = 0, SUCCESS = 1, FAILURE = 2 5 | int32 status 6 | --- 7 | #feedback 8 | int32 status 9 | -------------------------------------------------------------------------------- /behavior_tree_leaves/launch/test_behavior_tree.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /behavior_tree/package.xml: -------------------------------------------------------------------------------- 1 | 2 | behavior_tree 3 | 1.0.0 4 | A ROS behavior tree library. The leaf nodes (user defined) can be either in C++ or python 5 | 6 | Michele Colledanchise 7 | MIT 8 | Michele Colledanchise 9 | Rocco Santomo 10 | Petter Ögren 11 | 12 | catkin 13 | behavior_tree_core 14 | behavior_tree_leaves 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/python/class_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Author: Anas Abou Allaban 3 | 4 | import rospy 5 | from bt_action import BTAction 6 | 7 | 8 | class Start(BTAction): 9 | 10 | def __init__(self): 11 | self.condition = true 12 | # Behavior tree action server 13 | super(Start, self).__init__('Start') 14 | 15 | def execute_cb(self, goal): 16 | while not rospy.is_shutdown(): 17 | if self._as.is_preempt_requested(): 18 | rospy.loginfo("{} halted".format(self._action_name)) 19 | self._as.set_preempted() 20 | self.set_status(False) 21 | break 22 | if condition: 23 | self.set_status(True) 24 | else: 25 | self.set_status(False) 26 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/cpp/class_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Example for abstract class implementation 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | 9 | class StartCondition: public BTAction 10 | { 11 | public: 12 | bool start_condition = false; 13 | ros::NodeHandle nh_; 14 | 15 | StartCondition(ros::NodeHandle* n) : 16 | BTAction("StartCondition"), 17 | nh_(*n) 18 | { 19 | nh_.param("/start_condition", start_condition, false); 20 | } 21 | 22 | void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal) 23 | { 24 | set_status(SUCCESS); 25 | // if (start_condition) 26 | // set_status(SUCCESS); 27 | // else 28 | // set_status(FAILURE); 29 | } 30 | }; 31 | 32 | int main(int argc, char** argv) 33 | { 34 | ros::init(argc, argv, "start_conditions"); 35 | ros::NodeHandle nh; 36 | StartCondition sc(&nh); 37 | ros::spin(); 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 - 2018 Michele Colledanchise 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /behavior_tree_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | behavior_tree_core 3 | 0.0.0 4 | 5 | This package provides a behavior trees framework in ROS. 6 | 7 | Michele Colledanchise 8 | MIT 9 | Michele Colledanchise 10 | 11 | catkin 12 | message_generation 13 | roscpp 14 | roslib 15 | rospy 16 | std_msgs 17 | message_generation 18 | actionlib 19 | actionlib_msgs 20 | glut 21 | libxmu-dev 22 | libxi-dev 23 | actionlib 24 | actionlib_msgs 25 | message_runtime 26 | roscpp 27 | roslib 28 | rospy 29 | std_msgs 30 | 31 | 32 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/python/bt_action.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Author: Anas Abou Allaban 3 | 4 | import rospy 5 | import actionlib 6 | import behavior_tree_core.msg 7 | from abc import ABCMeta, abstractmethod 8 | 9 | 10 | class BTAction: 11 | __metaclass__ = ABCMeta 12 | _feedback = behavior_tree_core.msg.BTFeedback() 13 | _result = behavior_tree_core.msg.BTResult() 14 | 15 | def __init__(self, name): 16 | # Behavior tree action server 17 | self._action_name = name 18 | self._as = actionlib.SimpleActionServer(self._action_name, behavior_tree_core.msg.BTAction, 19 | execute_cb=self.execute_cb, auto_start=False) 20 | self._as.start() 21 | rospy.loginfo("{} Server Started".format(self._action_name)) 22 | 23 | @abstractmethod 24 | def execute_cb(self, goal): 25 | raise NotImplementedError() 26 | 27 | def set_status(self, status): 28 | if status: 29 | self._feedback.status = 1 30 | self._result.status = self._feedback.status 31 | rospy.loginfo('Action %s: Succeeded' % self._action_name) 32 | self._as.set_succeeded(self._result) 33 | else: 34 | self._feedback.status = 2 35 | self._result.status = self._feedback.status 36 | rospy.loginfo('Action %s: Failed' % self._action_name) 37 | self._as.set_succeeded(self._result) 38 | -------------------------------------------------------------------------------- /behavior_tree_core/src/exceptions.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::BehaviorTreeException::BehaviorTreeException(const std::string Message) 17 | { 18 | this->Message = std::string("BehaviorTreeException: " + Message).c_str(); 19 | } 20 | 21 | const char* BT::BehaviorTreeException::what() 22 | { 23 | return Message; 24 | } 25 | -------------------------------------------------------------------------------- /behavior_tree_core/src/leaf_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 11 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | BT::LeafNode::LeafNode(std::string name) : TreeNode(name) {} 18 | 19 | BT::LeafNode::~LeafNode() {} 20 | 21 | 22 | void BT::LeafNode::ResetColorState() 23 | { 24 | color_status_ = BT::IDLE; 25 | } 26 | 27 | int BT::LeafNode::Depth() 28 | { 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /behavior_tree_core/include/leaf_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef LEAF_NODE_H 14 | #define LEAF_NODE_H 15 | 16 | #include 17 | #include 18 | #include 19 | namespace BT 20 | { 21 | class LeafNode : public TreeNode 22 | { 23 | protected: 24 | public: 25 | explicit LeafNode(std::string name); 26 | ~LeafNode(); 27 | void ResetColorState(); 28 | int Depth(); 29 | }; 30 | } // namespace BT 31 | 32 | #endif // LEAF_NODE_H 33 | -------------------------------------------------------------------------------- /behavior_tree_core/src/condition_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::ConditionNode::ConditionNode(std::string name) : LeafNode::LeafNode(name) 17 | { 18 | type_ = BT::CONDITION_NODE; 19 | } 20 | 21 | BT::ConditionNode::~ConditionNode() {} 22 | 23 | void BT::ConditionNode::Halt() {} 24 | 25 | int BT::ConditionNode::DrawType() 26 | { 27 | // Lock acquistion 28 | 29 | return BT::CONDITION; 30 | } 31 | -------------------------------------------------------------------------------- /behavior_tree_core/include/tick_engine.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef TICK_ENGINE_H 14 | #define TICK_ENGINE_H 15 | 16 | #include 17 | 18 | class TickEngine 19 | { 20 | private: 21 | int value_; 22 | std::mutex mutex_; 23 | std::condition_variable condition_variable_; 24 | public: 25 | explicit TickEngine(int initial_value); 26 | ~TickEngine(); 27 | void Wait(); 28 | void Tick(); 29 | }; 30 | 31 | #endif // TICK_ENGINE_H 32 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/python/condition_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | 4 | import rospy 5 | 6 | import actionlib 7 | 8 | import behavior_tree_core.msg 9 | 10 | 11 | 12 | 13 | class BTAction(object): 14 | # create messages that are used to publish feedback/result 15 | _feedback = behavior_tree_core.msg.BTFeedback() 16 | _result = behavior_tree_core.msg.BTResult() 17 | 18 | def __init__(self, name): 19 | self._action_name = name 20 | self._as = actionlib.SimpleActionServer(self._action_name, behavior_tree_core.msg.BTAction, execute_cb=self.execute_cb, auto_start = False) 21 | self._as.start() 22 | 23 | def execute_cb(self, goal): 24 | 25 | # publish info to the console for the user 26 | rospy.loginfo('Checking Condition') 27 | 28 | self.set_status('FAILURE') 29 | 30 | 31 | 32 | def set_status(self,status): 33 | if status == 'SUCCESS': 34 | self._feedback.status = 1 35 | self._result.status = self._feedback.status 36 | rospy.loginfo('Action %s: Succeeded' % self._action_name) 37 | self._as.set_succeeded(self._result) 38 | elif status == 'FAILURE': 39 | self._feedback.status = 2 40 | self._result.status = self._feedback.status 41 | rospy.loginfo('Action %s: Failed' % self._action_name) 42 | self._as.set_succeeded(self._result) 43 | else: 44 | rospy.logerr('Action %s: has a wrong return status' % self._action_name) 45 | 46 | 47 | 48 | if __name__ == '__main__': 49 | rospy.init_node('condition') 50 | BTAction(rospy.get_name()) 51 | rospy.spin() 52 | -------------------------------------------------------------------------------- /behavior_tree_core/include/exceptions.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef EXCEPTIONS_H 15 | #define EXCEPTIONS_H 16 | 17 | #include 18 | #include 19 | 20 | namespace BT 21 | { 22 | /// Exception class 23 | class BehaviorTreeException : public std::exception 24 | { 25 | private: 26 | const char* Message; 27 | public: 28 | explicit BehaviorTreeException(const std::string Message); 29 | const char* what(); 30 | }; 31 | } // namespace BT 32 | 33 | #endif // EXCEPTIONS_H 34 | -------------------------------------------------------------------------------- /behavior_tree_core/include/sequence_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef SEQUENCE_NODE_H 15 | #define SEQUENCE_NODE_H 16 | 17 | #include 18 | #include 19 | 20 | 21 | namespace BT 22 | { 23 | class SequenceNode : public ControlNode 24 | { 25 | public: 26 | // Constructor 27 | explicit SequenceNode(std::string name); 28 | ~SequenceNode(); 29 | int DrawType(); 30 | BT::ReturnStatus Tick(); 31 | }; 32 | } // namespace BT 33 | 34 | #endif // SEQUENCE_NODE_H 35 | -------------------------------------------------------------------------------- /behavior_tree_core/src/action_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | 17 | BT::ActionNode::ActionNode(std::string name) : LeafNode::LeafNode(name) 18 | { 19 | type_ = BT::ACTION_NODE; 20 | } 21 | 22 | BT::ActionNode::~ActionNode() {} 23 | 24 | 25 | BT::ReturnStatus BT::ActionNode::Tick() 26 | { 27 | return BT::EXIT; // not used in action node. 28 | } 29 | 30 | 31 | int BT::ActionNode::DrawType() 32 | { 33 | // Lock acquistion 34 | 35 | return BT::ACTION; 36 | } 37 | -------------------------------------------------------------------------------- /behavior_tree_core/include/decorators/negation_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef NEGATION_NODE_H 14 | #define NEGATION_NODE_H 15 | 16 | #include 17 | #include 18 | 19 | namespace BT 20 | { 21 | class NegationNode : public DecoratorNode 22 | { 23 | public: 24 | // Constructor 25 | explicit NegationNode(std::string name); 26 | ~NegationNode(); 27 | BT::ReturnStatus Tick(); 28 | BT::ReturnStatus convert(BT::ReturnStatus input); 29 | }; 30 | } // namespace BT 31 | 32 | #endif // NEGATION_NODE_H 33 | -------------------------------------------------------------------------------- /behavior_tree_core/include/fallback_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef FALLBACK_NODE_H 14 | #define FALLBACK_NODE_H 15 | 16 | #include 17 | #include 18 | 19 | namespace BT 20 | { 21 | class FallbackNode : public ControlNode 22 | { 23 | public: 24 | // Constructor 25 | explicit FallbackNode(std::string name); 26 | ~FallbackNode(); 27 | int DrawType(); 28 | BT::ReturnStatus Tick(); // The method that is going to be executed what the node is ticked 29 | }; 30 | } // namespace BT 31 | 32 | #endif // FALLBACK_NODE_H 33 | -------------------------------------------------------------------------------- /behavior_tree_core/include/decorator_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef DECORATOR_NODE_H 14 | #define DECORATOR_NODE_H 15 | 16 | #include 17 | #include 18 | 19 | namespace BT 20 | { 21 | class DecoratorNode : public ControlNode 22 | { 23 | public: 24 | // Constructor 25 | explicit DecoratorNode(std::string name); 26 | ~DecoratorNode(); 27 | void AddChild(TreeNode* child); 28 | int DrawType(); 29 | BT::ReturnStatus Tick(); // The method that is going to be executed what the node is ticked 30 | }; 31 | } // namespace BT 32 | 33 | #endif // DECORATOR_NODE_H 34 | -------------------------------------------------------------------------------- /behavior_tree_core/templates/condition_node_template.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) (YEAR) YOUR NAME- All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | BT::CLASSNAME::CONSTRUCTOR(std::string name) : ConditionNode::ConditionNode(name) 18 | { 19 | } 20 | 21 | BT::CLASSNAME::~CONSTRUCTOR() {} 22 | 23 | BT::ReturnStatus BT::CLASSNAME::Tick() 24 | { 25 | // Condition checking and state update 26 | 27 | if (/*CONDITION TO CHECK*/) 28 | { 29 | set_status(BT::SUCCESS); 30 | return BT::SUCCESS; 31 | } 32 | else 33 | { 34 | set_status(BT::FAILURE); 35 | return BT::FAILURE; 36 | } 37 | } 38 | 39 | -------------------------------------------------------------------------------- /behavior_tree_core/include/conditions/condition_test_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef CONDITIONS_CONDITION_TEST_NODE_H 15 | #define CONDITIONS_CONDITION_TEST_NODE_H 16 | 17 | #include 18 | #include 19 | 20 | namespace BT 21 | { 22 | class ConditionTestNode : public ConditionNode 23 | { 24 | public: 25 | // Constructor 26 | explicit ConditionTestNode(std::string Name); 27 | ~ConditionTestNode(); 28 | void set_boolean_value(bool boolean_value); 29 | 30 | BT::ReturnStatus Tick(); 31 | private: 32 | bool boolean_value_; 33 | }; 34 | } // namespace BT 35 | 36 | #endif // CONDITIONS_CONDITION_TEST_NODE_H 37 | -------------------------------------------------------------------------------- /behavior_tree_core/include/actions/action_test_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef ACTIONS_ACTION_TEST_NODE_H 15 | #define ACTIONS_ACTION_TEST_NODE_H 16 | 17 | #include 18 | #include 19 | namespace BT 20 | { 21 | class ActionTestNode : public ActionNode 22 | { 23 | public: 24 | // Constructor 25 | explicit ActionTestNode(std::string Name); 26 | ~ActionTestNode(); 27 | 28 | void WaitForTick(); 29 | void set_time(int time); 30 | 31 | void Halt(); 32 | void set_boolean_value(bool boolean_value); 33 | private: 34 | int time_; 35 | bool boolean_value_; 36 | }; 37 | } // namespace BT 38 | 39 | #endif // ACTIONS_ACTION_TEST_NODE_H 40 | -------------------------------------------------------------------------------- /behavior_tree_core/include/fallback_node_with_memory.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef FALLBACK_NODE_WITH_MEMORY_H 14 | #define FALLBACK_NODE_WITH_MEMORY_H 15 | 16 | #include 17 | #include 18 | 19 | namespace BT 20 | { 21 | class FallbackNodeWithMemory : public ControlNode 22 | { 23 | public: 24 | // Constructor 25 | explicit FallbackNodeWithMemory(std::string name); 26 | FallbackNodeWithMemory(std::string name, int reset_policy); 27 | ~FallbackNodeWithMemory(); 28 | int DrawType(); 29 | BT::ReturnStatus Tick(); 30 | void Halt(); 31 | private: 32 | unsigned int current_child_idx_; 33 | unsigned int reset_policy_; 34 | }; 35 | } // namespace BT 36 | 37 | 38 | #endif // FALLBACK_NODE_WITH_MEMORY_H 39 | -------------------------------------------------------------------------------- /behavior_tree_core/include/parallel_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef PARALLEL_NODE_H 15 | #define PARALLEL_NODE_H 16 | 17 | #include 18 | #include 19 | 20 | namespace BT 21 | { 22 | class ParallelNode : public ControlNode 23 | { 24 | public: 25 | // Constructor 26 | explicit ParallelNode(std::string name, int threshold_M); 27 | ~ParallelNode(); 28 | int DrawType(); 29 | BT::ReturnStatus Tick(); 30 | void Halt(); 31 | 32 | unsigned int get_threshold_M(); 33 | void set_threshold_M(unsigned int threshold_M); 34 | 35 | private: 36 | unsigned int threshold_M_; 37 | unsigned int success_childred_num_; 38 | unsigned int failure_childred_num_; 39 | }; 40 | } // namespace BT 41 | #endif // PARALLEL_NODE_H 42 | -------------------------------------------------------------------------------- /behavior_tree_core/include/sequence_node_with_memory.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef SEQUENCE_NODE_WITH_MEMORY_H 15 | #define SEQUENCE_NODE_WITH_MEMORY_H 16 | 17 | 18 | #include 19 | #include 20 | 21 | namespace BT 22 | { 23 | class SequenceNodeWithMemory : public ControlNode 24 | { 25 | public: 26 | // Constructor 27 | explicit SequenceNodeWithMemory(std::string name); 28 | SequenceNodeWithMemory(std::string name, int reset_policy); 29 | ~SequenceNodeWithMemory(); 30 | int DrawType(); 31 | // The method that is going to be executed by the thread 32 | BT::ReturnStatus Tick(); 33 | void Halt(); 34 | private: 35 | unsigned int current_child_idx_; 36 | unsigned int reset_policy_; 37 | }; 38 | } // namespace BT 39 | 40 | #endif // SEQUENCE_NODE_WITH_MEMORY_H 41 | -------------------------------------------------------------------------------- /behavior_tree_core/include/condition_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef CONDITION_NODE_H 15 | #define CONDITION_NODE_H 16 | 17 | #include 18 | #include 19 | 20 | namespace BT 21 | { 22 | class ConditionNode : public LeafNode 23 | { 24 | public: 25 | // Constructor 26 | explicit ConditionNode(std::string name); 27 | ~ConditionNode(); 28 | 29 | // The method that is going to be executed by the thread 30 | virtual BT::ReturnStatus Tick() = 0; 31 | 32 | // The method used to interrupt the execution of the node 33 | void Halt(); 34 | 35 | // Methods used to access the node state without the 36 | // conditional waiting (only mutual access) 37 | bool WriteState(ReturnStatus new_state); 38 | int DrawType(); 39 | }; 40 | } // namespace BT 41 | 42 | #endif // CONDITION_NODE_H 43 | -------------------------------------------------------------------------------- /behavior_tree_core/include/action_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef ACTION_NODE_H 14 | #define ACTION_NODE_H 15 | 16 | #include 17 | #include 18 | namespace BT 19 | { 20 | 21 | class ActionNode : public LeafNode 22 | { 23 | public: 24 | // Constructor 25 | explicit ActionNode(std::string name); 26 | ~ActionNode(); 27 | 28 | // The method that is going to be executed by the thread 29 | virtual void WaitForTick() = 0; 30 | BT::ReturnStatus Tick(); 31 | 32 | // The method used to interrupt the execution of the node 33 | virtual void Halt() = 0; 34 | 35 | // Methods used to access the node state without the 36 | // conditional waiting (only mutual access) 37 | bool WriteState(ReturnStatus new_state); 38 | int DrawType(); 39 | }; 40 | } // namespace BT 41 | 42 | #endif // ACTION_NODE_H 43 | -------------------------------------------------------------------------------- /behavior_tree_core/include/conditions/ros_condition.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef CONDITIONS_ROS_CONDITION_H 15 | #define CONDITIONS_ROS_CONDITION_H 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace BT 25 | { 26 | class ROSCondition : public ConditionNode 27 | { 28 | protected: 29 | actionlib::SimpleActionClient action_client_; 30 | behavior_tree_core::BTResult node_result; 31 | behavior_tree_core::BTGoal goal; 32 | public: 33 | // Constructor 34 | explicit ROSCondition(std::string Name); 35 | ~ROSCondition(); 36 | 37 | ReturnStatus Tick(); 38 | }; 39 | } // namespace BT 40 | 41 | #endif // CONDITIONS_ROS_CONDITION_H 42 | -------------------------------------------------------------------------------- /behavior_tree_core/src/tick_engine.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | 15 | TickEngine::TickEngine(int initial_value) 16 | { 17 | value_ = initial_value; 18 | } 19 | 20 | TickEngine::~TickEngine() {} 21 | 22 | void TickEngine::Wait() 23 | { 24 | // Lock acquire (need a unique lock for the condition variable usage) 25 | std::unique_lock UniqueLock(mutex_); 26 | 27 | // If the state is 0 then we have to wait for a signal 28 | if (value_ == 0) 29 | condition_variable_.wait(UniqueLock); 30 | 31 | // Once here we decrement the state 32 | value_--; 33 | } 34 | 35 | void TickEngine::Tick() 36 | { 37 | // Lock acquire 38 | 39 | std::lock_guard LockGuard(mutex_); 40 | 41 | // State increment 42 | value_++; 43 | 44 | // Notification 45 | condition_variable_.notify_all(); 46 | } 47 | -------------------------------------------------------------------------------- /behavior_tree_core/include/behavior_tree.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef BEHAVIOR_TREE_H 14 | #define BEHAVIOR_TREE_H 15 | 16 | 17 | 18 | 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include /* pow */ 46 | 47 | #include "ros/ros.h" 48 | #include "std_msgs/UInt8.h" 49 | 50 | void Execute(BT::ControlNode* root, int TickPeriod_milliseconds); 51 | 52 | 53 | #endif // BEHAVIOR_TREE_H 54 | -------------------------------------------------------------------------------- /behavior_tree_core/src/gtest/external_ros_nodes_test.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | 16 | int main(int argc, char **argv) 17 | { 18 | ros::init(argc, argv, "BehaviorTree"); 19 | try 20 | { 21 | int TickPeriod_milliseconds = 1000; 22 | 23 | 24 | BT::ROSAction* action = new BT::ROSAction("action"); 25 | BT::NegationNode* decorator = new BT::NegationNode("decorator"); 26 | BT::ROSCondition* condition = new BT::ROSCondition("condition"); 27 | 28 | 29 | BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1"); 30 | 31 | sequence1->AddChild(decorator); 32 | decorator->AddChild(condition); 33 | sequence1->AddChild(action); 34 | 35 | Execute(sequence1, TickPeriod_milliseconds); // from BehaviorTree.cpp 36 | } 37 | catch (BT::BehaviorTreeException& Exception) 38 | { 39 | std::cout << Exception.what() << std::endl; 40 | } 41 | 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /behavior_tree_core/include/actions/ros_action.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef ACTIONS_ROS_ACTION_H 15 | #define ACTIONS_ROS_ACTION_H 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | 26 | namespace BT 27 | { 28 | class ROSAction : public ActionNode 29 | { 30 | protected: 31 | actionlib::SimpleActionClient action_client_; 32 | behavior_tree_core::BTResult node_result; 33 | behavior_tree_core::BTGoal goal; 34 | public: 35 | // Constructor 36 | explicit ROSAction(std::string name); 37 | ~ROSAction(); 38 | 39 | // The method that is going to be executed by the thread 40 | void WaitForTick(); 41 | 42 | // The method used to interrupt the execution of the node 43 | void Halt(); 44 | }; 45 | } // namespace BT 46 | 47 | #endif // ACTIONS_ROS_ACTION_H 48 | -------------------------------------------------------------------------------- /behavior_tree_core/src/conditions/ros_condition.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | enum Status {RUNNING, SUCCESS, FAILURE}; 18 | 19 | 20 | BT::ROSCondition::ROSCondition(std::string name) : 21 | ConditionNode::ConditionNode(name), 22 | action_client_(name, true) 23 | { 24 | ROS_INFO("Waiting For the Acutator named %s to start", get_name().c_str()); 25 | action_client_.waitForServer(); // will wait for infinite time until the server starts 26 | ROS_INFO("Actuator %s Started", get_name().c_str()); 27 | } 28 | 29 | BT::ROSCondition::~ROSCondition() {} 30 | 31 | BT::ReturnStatus BT::ROSCondition::Tick() 32 | { 33 | ROS_INFO("I am running the request"); 34 | 35 | // Condition checking and state update 36 | action_client_.sendGoal(goal); 37 | action_client_.waitForResult(ros::Duration(30.0)); 38 | node_result = *(action_client_.getResult()); 39 | set_status((ReturnStatus)node_result.status); 40 | return (ReturnStatus)node_result.status; 41 | } 42 | -------------------------------------------------------------------------------- /behavior_tree_core/include/draw.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef DRAW_H 14 | #define DRAW_H 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | void drawEllipse(float xradius, float yradius); 24 | 25 | void drawTree(BT::ControlNode* tree_); 26 | 27 | void draw_status(float x, float y, int node_status); 28 | 29 | void drawString(void * font, const char *string, float x, float y, float z); 30 | 31 | void renderBitmapString(float x, float y, void *font, const char *string); 32 | 33 | void draw_node(float x, float y, int node_type, const char *leafName, int status); 34 | 35 | void draw_edge(GLfloat parent_x, GLfloat parent_y, GLfloat parent_size, 36 | GLfloat child_x, GLfloat child_y, GLfloat child_size); 37 | 38 | void keyboard(unsigned char key, int x, int y); 39 | 40 | void drawCircle(float radius); 41 | 42 | int compute_node_lines(const char *string); 43 | 44 | int compute_max_width(const char *string); 45 | 46 | #endif // DRAW_H 47 | -------------------------------------------------------------------------------- /behavior_tree_core/src/behavior_tree.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | 20 | void Execute(BT::ControlNode* root, int TickPeriod_milliseconds) 21 | { 22 | std::cout << "Start Drawing!" << std::endl; 23 | // Starts in another thread the drawing of the BT 24 | std::thread t2(&drawTree, root); 25 | t2.detach(); 26 | BT::DotBt dotbt(root); 27 | std::thread t(&BT::DotBt::publish, dotbt); 28 | 29 | root->ResetColorState(); 30 | 31 | while (ros::ok()) 32 | { 33 | DEBUG_STDOUT("Ticking the root node !"); 34 | 35 | // Ticking the root node 36 | root->Tick(); 37 | // Printing its state 38 | // root->GetNodeState(); 39 | 40 | if (root->get_status() != BT::RUNNING) 41 | { 42 | // when the root returns a status it resets the colors of the tree 43 | root->ResetColorState(); 44 | } 45 | std::this_thread::sleep_for(std::chrono::milliseconds(TickPeriod_milliseconds)); 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/python/action_example.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | 4 | import rospy 5 | 6 | import actionlib 7 | 8 | import behavior_tree_core.msg 9 | 10 | 11 | 12 | 13 | class BTAction(object): 14 | # create messages that are used to publish feedback/result 15 | _feedback = behavior_tree_core.msg.BTFeedback() 16 | _result = behavior_tree_core.msg.BTResult() 17 | 18 | def __init__(self, name): 19 | self._action_name = name 20 | self._as = actionlib.SimpleActionServer(self._action_name, behavior_tree_core.msg.BTAction, execute_cb=self.execute_cb, auto_start = False) 21 | self._as.start() 22 | 23 | def execute_cb(self, goal): 24 | # helper variables 25 | r = rospy.Rate(1) 26 | success = True 27 | 28 | 29 | 30 | # publish info to the console for the user 31 | rospy.loginfo('Starting Action') 32 | 33 | # start executing the action 34 | for i in xrange(1, 5): 35 | # check that preempt has not been requested by the client 36 | if self._as.is_preempt_requested(): 37 | rospy.loginfo('Action Halted') 38 | self._as.set_preempted() 39 | success = False 40 | break 41 | 42 | rospy.loginfo('Executing Action') 43 | 44 | # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes 45 | r.sleep() 46 | 47 | if success: 48 | self.set_status('FAILURE') 49 | 50 | 51 | 52 | def set_status(self,status): 53 | if status == 'SUCCESS': 54 | self._feedback.status = 1 55 | self._result.status = self._feedback.status 56 | rospy.loginfo('Action %s: Succeeded' % self._action_name) 57 | self._as.set_succeeded(self._result) 58 | elif status == 'FAILURE': 59 | self._feedback.status = 2 60 | self._result.status = self._feedback.status 61 | rospy.loginfo('Action %s: Failed' % self._action_name) 62 | self._as.set_succeeded(self._result) 63 | else: 64 | rospy.logerr('Action %s: has a wrong return status' % self._action_name) 65 | 66 | 67 | 68 | if __name__ == '__main__': 69 | rospy.init_node('action') 70 | BTAction(rospy.get_name()) 71 | rospy.spin() 72 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/cpp/bt_action.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Abstract class used to create different BT actions. 3 | * Usage: Pass the action's name to this class when constructing your action. 4 | * Nodehandles and other variables should be part of your implementation. 5 | * Maintainer: Anas Abou Allaban 6 | */ 7 | 8 | #ifndef BTACTION_CLASS 9 | #define BTACTION_CLASS 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include // std::bind 16 | 17 | enum Status {RUNNING, SUCCESS, FAILURE}; // BT return statuses 18 | 19 | class BTAction 20 | { 21 | protected: 22 | // Create action server 23 | actionlib::SimpleActionServer as_; 24 | std::string action_name_; 25 | behavior_tree_core::BTFeedback feedback_; // Action feedback (SUCCESS, FAILURE) 26 | behavior_tree_core::BTResult result_; // Action feedback (feedback for us) 27 | 28 | public: 29 | BTAction(std::string name); 30 | ~BTAction(void) {} 31 | virtual void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal) = 0; 32 | void set_status(int status); // Returns the status to the client (Behavior Tree) 33 | 34 | }; 35 | 36 | BTAction::BTAction(std::string name): 37 | as_(name, std::bind(&BTAction::execute_callback, this, _1), false), 38 | action_name_(name) 39 | { 40 | as_.start(); // Starts the action server 41 | } 42 | 43 | void BTAction::set_status(int status) 44 | { 45 | // Set The feedback and result of BT.action 46 | feedback_.status = status; 47 | result_.status = feedback_.status; 48 | as_.publishFeedback(feedback_); // Publish feedback 49 | // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE). 50 | as_.setSucceeded(result_); 51 | 52 | switch (status) // Print for convenience 53 | { 54 | case SUCCESS: 55 | ROS_INFO("%s Succeeded", ros::this_node::getName().c_str() ); 56 | break; 57 | case FAILURE: 58 | ROS_INFO("%s Failed", ros::this_node::getName().c_str() ); 59 | break; 60 | default: 61 | break; 62 | } 63 | } 64 | 65 | #endif // BTACTION_CLASS 66 | -------------------------------------------------------------------------------- /behavior_tree_core/templates/action_node_template.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) (YEAR) YOUR NAME- All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | BT::CLASSNAME::CONSTRUCTOR(std::string name) : ActionNode::ActionNode(name) 17 | { 18 | thread_ = std::thread(&ActionTestNode::WaitForTick, this); 19 | } 20 | 21 | BT::CLASSNAME::~CONSTRUCTOR() {} 22 | 23 | void BT::CLASSNAME::WaitForTick() 24 | { 25 | while (true) 26 | { 27 | // Waiting for the first tick to come 28 | DEBUG_STDOUT(get_name() << " WAIT FOR TICK"); 29 | 30 | tick_engine.Wait(); 31 | DEBUG_STDOUT(get_name() << " TICK RECEIVED"); 32 | 33 | // Running state 34 | set_status(BT::RUNNING); 35 | // Perform action... 36 | 37 | while (get_status() != BT::HALTED) 38 | { 39 | /*HERE THE CODE TO EXECUTE FOR THE ACTION. 40 | wHEN THE ACTION HAS FINISHED CORRECLTY, CALL set_status(BT::SUCCESS) 41 | IF THE ACTION FAILS, CALL set_status(BT::FAILURE)*/ 42 | } 43 | } 44 | } 45 | 46 | void BT::CLASSNAME::Halt() 47 | { 48 | /*HERE THE CODE TO PERFORM WHEN THE ACTION IS HALTED*/ 49 | set_status(BT::HALTED); 50 | DEBUG_STDOUT("HALTED state set!"); 51 | } 52 | 53 | 54 | -------------------------------------------------------------------------------- /behavior_tree_core/src/conditions/condition_test_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | BT::ConditionTestNode::ConditionTestNode(std::string name) : ConditionNode::ConditionNode(name) 18 | { 19 | type_ = BT::CONDITION_NODE; 20 | boolean_value_ = true; 21 | } 22 | 23 | BT::ConditionTestNode::~ConditionTestNode() {} 24 | 25 | BT::ReturnStatus BT::ConditionTestNode::Tick() 26 | { 27 | if (get_status() == BT::EXIT) 28 | { 29 | // The behavior tree is going to be destroied 30 | return BT::EXIT; 31 | } 32 | 33 | // Condition checking and state update 34 | 35 | if (boolean_value_) 36 | { 37 | set_status(BT::SUCCESS); 38 | std::cout << get_name() << " returning Success" << BT::SUCCESS << "!" << std::endl; 39 | return BT::SUCCESS; 40 | } 41 | else 42 | { 43 | set_status(BT::FAILURE); 44 | std::cout << get_name() << " returning Failure" << BT::FAILURE << "!" << std::endl; 45 | return BT::FAILURE; 46 | } 47 | } 48 | 49 | 50 | 51 | 52 | void BT::ConditionTestNode::set_boolean_value(bool boolean_value) 53 | { 54 | boolean_value_ = boolean_value; 55 | } 56 | 57 | -------------------------------------------------------------------------------- /behavior_tree_core/include/control_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #ifndef CONTROL_NODE_H 14 | #define CONTROL_NODE_H 15 | 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace BT 22 | { 23 | class ControlNode : public TreeNode 24 | { 25 | protected: 26 | // Children vector 27 | std::vector children_nodes_; 28 | 29 | // Children states 30 | std::vector children_states_; 31 | 32 | // Vector size 33 | unsigned int N_of_children_; 34 | // child i status. Used to rout the ticks 35 | ReturnStatus child_i_status_; 36 | 37 | public: 38 | // Constructor 39 | explicit ControlNode(std::string name); 40 | ~ControlNode(); 41 | 42 | // The method used to fill the child vector 43 | void AddChild(TreeNode* child); 44 | 45 | // The method used to know the number of children 46 | unsigned int GetChildrenNumber(); 47 | std::vector GetChildren(); 48 | // The method used to interrupt the execution of the node 49 | void Halt(); 50 | void ResetColorState(); 51 | void HaltChildren(int i); 52 | int Depth(); 53 | 54 | // Methods used to access the node state without the 55 | // conditional waiting (only mutual access) 56 | bool WriteState(ReturnStatus new_state); 57 | }; 58 | } // namespace BT 59 | 60 | #endif // CONTROL_NODE_H 61 | -------------------------------------------------------------------------------- /behavior_tree_core/src/tree.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | 16 | 17 | int main(int argc, char **argv) 18 | { 19 | ros::init(argc, argv, "BehaviorTree"); 20 | try 21 | { 22 | int TickPeriod_milliseconds = 1000; 23 | 24 | BT::ActionTestNode* action1 = new BT::ActionTestNode("Action 1"); 25 | BT::ConditionTestNode* condition1 = new BT::ConditionTestNode("Condition 1"); 26 | BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1"); 27 | 28 | BT::ActionTestNode* action2 = new BT::ActionTestNode("Action 2"); 29 | BT::ConditionTestNode* condition2 = new BT::ConditionTestNode("Condition 2"); 30 | BT:: SequenceNode* sequence2 = new BT::SequenceNode("seq1"); 31 | 32 | BT::ActionTestNode* action3 = new BT::ActionTestNode("Action 3"); 33 | BT::ConditionTestNode* condition3 = new BT::ConditionTestNode("Condition 3"); 34 | BT:: SequenceNode* sequence3 = new BT::SequenceNode("seq1"); 35 | 36 | 37 | sequence1->AddChild(sequence2); 38 | sequence1->AddChild(condition1); 39 | sequence1->AddChild(action1); 40 | sequence1->AddChild(sequence3); 41 | 42 | sequence2->AddChild(action2); 43 | sequence2->AddChild(condition2); 44 | 45 | sequence3->AddChild(condition3); 46 | sequence3->AddChild(action3); 47 | 48 | Execute(sequence1, TickPeriod_milliseconds); // from BehaviorTree.cpp 49 | } 50 | catch (BT::BehaviorTreeException& Exception) 51 | { 52 | std::cout << Exception.what() << std::endl; 53 | } 54 | 55 | return 0; 56 | } 57 | 58 | 59 | -------------------------------------------------------------------------------- /behavior_tree_leaves/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | behavior_tree_leaves 4 | 0.0.0 5 | The behavior_tree_actions package 6 | 7 | 8 | 9 | 10 | Michele Colledanchise 11 | 12 | Michele Colledanchise 13 | 14 | 15 | 16 | 17 | MIT 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | actionlib 44 | actionlib_msgs 45 | message_generation 46 | roscpp 47 | rospy 48 | roslib 49 | std_msgs 50 | tf 51 | behavior_tree_core 52 | behavior_tree_core 53 | tf 54 | actionlib 55 | actionlib_msgs 56 | roscpp 57 | rospy 58 | roslib 59 | std_msgs 60 | message_runtime 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /behavior_tree_core/src/actions/action_test_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | 18 | BT::ActionTestNode::ActionTestNode(std::string name) : ActionNode::ActionNode(name) 19 | { 20 | type_ = BT::ACTION_NODE; 21 | boolean_value_ = true; 22 | time_ = 3; 23 | thread_ = std::thread(&ActionTestNode::WaitForTick, this); 24 | } 25 | 26 | BT::ActionTestNode::~ActionTestNode() {} 27 | 28 | void BT::ActionTestNode::WaitForTick() 29 | { 30 | while (true) 31 | { 32 | // Waiting for the first tick to come 33 | DEBUG_STDOUT(get_name() << " WAIT FOR TICK"); 34 | 35 | tick_engine.Wait(); 36 | DEBUG_STDOUT(get_name() << " TICK RECEIVED"); 37 | 38 | // Running state 39 | set_status(BT::RUNNING); 40 | // Perform action... 41 | int i = 0; 42 | while (get_status() != BT::HALTED && i++ < time_) 43 | { 44 | DEBUG_STDOUT(" Action " << get_name() << "running! Thread id:" << std::this_thread::get_id()); 45 | std::this_thread::sleep_for(std::chrono::seconds(1)); 46 | } 47 | if (get_status() != BT::HALTED) 48 | { 49 | if (boolean_value_) 50 | { 51 | set_status(BT::SUCCESS); 52 | DEBUG_STDOUT(" Action " << get_name() << " Done!"); 53 | } 54 | else 55 | { 56 | set_status(BT::FAILURE); 57 | DEBUG_STDOUT(" Action " << get_name() << " FAILED!"); 58 | } 59 | } 60 | } 61 | } 62 | 63 | void BT::ActionTestNode::Halt() 64 | { 65 | set_status(BT::HALTED); 66 | DEBUG_STDOUT("HALTED state set!"); 67 | } 68 | 69 | 70 | void BT::ActionTestNode::set_time(int time) 71 | { 72 | time_ = time; 73 | } 74 | 75 | 76 | 77 | void BT::ActionTestNode::set_boolean_value(bool boolean_value) 78 | { 79 | boolean_value_ = boolean_value; 80 | } 81 | 82 | 83 | -------------------------------------------------------------------------------- /behavior_tree_leaves/src/action_client.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | int main(int argc, char **argv) 20 | { 21 | ros::init(argc, argv, "test_action"); 22 | 23 | // create the action client 24 | // true causes the client to spin its own thread 25 | actionlib::SimpleActionClient ac("action", true); 26 | behavior_tree_core::BTResult node_result; 27 | ROS_INFO("Waiting for action server to start."); 28 | // wait for the action server to start 29 | ac.waitForServer(); // will wait for infinite time 30 | 31 | behavior_tree_core::BTGoal goal; 32 | 33 | 34 | int command = 0; 35 | bool isRunning = false; 36 | 37 | 38 | while (command != 3) 39 | { 40 | ROS_INFO("Send a command: 1:start the action | 2:stop the action | 3:exit the program"); 41 | std::cin >> command; 42 | 43 | switch (command) 44 | { 45 | case 1: 46 | if (!isRunning) 47 | { 48 | ROS_INFO("I am running the request"); 49 | ac.sendGoal(goal); 50 | isRunning = true; 51 | node_result = *(ac.getResult()); 52 | ROS_INFO("Action finished, status: %d", node_result.status); 53 | } 54 | else 55 | { 56 | ROS_INFO("I am re-running the request"); 57 | ac.cancelGoal(); 58 | ac.sendGoal(goal); 59 | ROS_INFO("Action finished, status: %d", node_result.status); 60 | } 61 | break; 62 | case 2: 63 | ROS_INFO("I am cancelling the request"); 64 | ac.cancelGoal(); 65 | isRunning = false; 66 | break; 67 | default: 68 | break; 69 | } 70 | } 71 | return 0; 72 | } 73 | 74 | -------------------------------------------------------------------------------- /behavior_tree_core/src/actions/ros_action.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | 18 | BT::ROSAction::ROSAction(std::string name) : 19 | ActionNode::ActionNode(name), 20 | action_client_(name, true) 21 | { 22 | actionlib::SimpleActionClient action_client_(get_name(), true); 23 | ROS_INFO("Waiting For the Acutator named %s to start", get_name().c_str()); 24 | action_client_.waitForServer(); // will wait for infinite time until the server starts 25 | ROS_INFO("The Acutator %s has started", get_name().c_str()); 26 | // thread_ start 27 | thread_ = std::thread(&ROSAction::WaitForTick, this); 28 | } 29 | 30 | BT::ROSAction::~ROSAction() {} 31 | 32 | void BT::ROSAction::WaitForTick() 33 | { 34 | while (true) 35 | { 36 | // Waiting for a tick to come 37 | tick_engine.Wait(); 38 | 39 | // Running state 40 | node_result.status = BT::RUNNING; 41 | set_status(BT::RUNNING); 42 | // Perform action... 43 | ROS_INFO("I am running the request to %s", get_name().c_str()); 44 | action_client_.sendGoal(goal); 45 | do 46 | { 47 | node_result = *(action_client_.getResult()); // checking the result 48 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 49 | } 50 | while (node_result.status == BT::RUNNING && get_status() != BT::HALTED); 51 | 52 | if (get_status() == BT::HALTED) 53 | { 54 | ROS_INFO("The Node is Halted"); 55 | ROS_INFO("I am Halting the client"); 56 | action_client_.cancelGoal(); 57 | } 58 | else 59 | { 60 | ROS_INFO("The Server Has Replied"); 61 | // Set this node status according to what the external node replied 62 | set_status((ReturnStatus)node_result.status); 63 | } 64 | } 65 | } 66 | 67 | 68 | void BT::ROSAction::Halt() 69 | { 70 | set_status(BT::HALTED); 71 | } 72 | -------------------------------------------------------------------------------- /behavior_tree_leaves/src/condition_client.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | int main(int argc, char **argv) 20 | { 21 | ros::init(argc, argv, "test_condition"); 22 | 23 | // create the action client 24 | // true causes the client to spin its own thread 25 | actionlib::SimpleActionClient ac("condition", true); 26 | behavior_tree_core::BTResult node_result; 27 | ROS_INFO("Waiting for action server to start."); 28 | // wait for the action server to start 29 | ac.waitForServer(); // will wait for infinite time 30 | 31 | behavior_tree_core::BTGoal goal; 32 | 33 | int command = 0; 34 | bool isRunning = false; 35 | 36 | 37 | 38 | while (command != 3) 39 | { 40 | ROS_INFO("Send a command: 1:Evaluate the condition | 3:exit the program"); 41 | std::cin >> command; 42 | 43 | switch (command) 44 | { 45 | case 1: 46 | if (!isRunning) 47 | { 48 | ROS_INFO("I am running the request"); 49 | ac.sendGoal(goal); 50 | isRunning = true; 51 | ac.waitForResult(ros::Duration(30.0)); 52 | node_result = *(ac.getResult()); 53 | ROS_INFO("Condition evaluated, status: %d", node_result.status); 54 | } 55 | else 56 | { 57 | ROS_INFO("I am re-running the request"); 58 | ac.cancelGoal(); 59 | ac.sendGoal(goal); 60 | ac.waitForResult(ros::Duration(30.0)); 61 | node_result = *(ac.getResult()); 62 | 63 | ROS_INFO("Condition evaluated, status: %d", node_result.status); 64 | } 65 | break; 66 | case 2: 67 | break; 68 | default: 69 | break; 70 | } 71 | } 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /behavior_tree_core/src/tree_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | 17 | BT::TreeNode::TreeNode(std::string name) : tick_engine(0) 18 | { 19 | // Initialization 20 | name_ = name; 21 | has_parent_ = false; 22 | is_state_updated_ = false; 23 | set_status(BT::IDLE); 24 | } 25 | 26 | BT::TreeNode::~TreeNode() {} 27 | 28 | void BT::TreeNode::set_status(ReturnStatus new_status) 29 | { 30 | if (new_status != BT::IDLE) 31 | { 32 | set_color_status(new_status); 33 | } 34 | 35 | // Lock acquistion 36 | std::unique_lock UniqueLock(state_mutex_); 37 | 38 | // state_ update 39 | status_ = new_status; 40 | } 41 | 42 | BT::ReturnStatus BT::TreeNode::get_status() 43 | { 44 | // Lock acquistion 45 | DEBUG_STDOUT(get_name() << " is setting its status to " << status_); 46 | 47 | std::lock_guard LockGuard(state_mutex_); 48 | 49 | return status_; 50 | } 51 | 52 | BT::ReturnStatus BT::TreeNode::get_color_status() 53 | { 54 | // Lock acquistion 55 | std::lock_guard LockGuard(color_state_mutex_); 56 | 57 | return color_status_; 58 | } 59 | 60 | void BT::TreeNode::set_color_status(ReturnStatus new_color_status) 61 | { 62 | // Lock acquistion 63 | std::lock_guard LockGuard(color_state_mutex_); 64 | // state_ update 65 | color_status_ = new_color_status; 66 | } 67 | 68 | float BT::TreeNode::get_x_pose() 69 | { 70 | return x_pose_; 71 | } 72 | 73 | void BT::TreeNode::set_x_pose(float x_pose) 74 | { 75 | x_pose_ = x_pose; 76 | } 77 | 78 | 79 | float BT::TreeNode::get_x_shift() 80 | { 81 | return x_shift_; 82 | } 83 | 84 | void BT::TreeNode::set_x_shift(float x_shift) 85 | { 86 | x_shift_ = x_shift; 87 | } 88 | 89 | void BT::TreeNode::set_name(std::string new_name) 90 | { 91 | name_ = new_name; 92 | } 93 | 94 | std::string BT::TreeNode::get_name() 95 | { 96 | return name_; 97 | } 98 | 99 | 100 | BT::NodeType BT::TreeNode::get_type() 101 | { 102 | return type_; 103 | } 104 | 105 | bool BT::TreeNode::has_parent() 106 | { 107 | return has_parent_; 108 | } 109 | 110 | void BT::TreeNode::set_has_parent(bool value) 111 | { 112 | has_parent_ = value; 113 | } 114 | -------------------------------------------------------------------------------- /behavior_tree_core/src/decorators/negation_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::NegationNode::NegationNode(std::string name) : DecoratorNode::DecoratorNode(name) {} 17 | 18 | BT::NegationNode::~NegationNode() {} 19 | 20 | BT::ReturnStatus BT::NegationNode::Tick() 21 | { 22 | { 23 | // gets the number of children. The number could change if, at runtime, one edits the tree. 24 | N_of_children_ = children_nodes_.size(); 25 | 26 | if (N_of_children_ > 0) 27 | { 28 | if (children_nodes_[0]->get_type() == BT::ACTION_NODE) 29 | { 30 | child_i_status_ = children_nodes_[0]->get_status(); 31 | 32 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 33 | { 34 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[0]->get_name()); 35 | children_nodes_[0]->tick_engine.Tick(); 36 | 37 | // waits for the tick to arrive to the child 38 | do 39 | { 40 | child_i_status_ = children_nodes_[0]->get_status(); 41 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 42 | } 43 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS && child_i_status_ != BT::FAILURE); 44 | } 45 | } 46 | else 47 | { 48 | child_i_status_ = children_nodes_[0]->Tick(); 49 | } 50 | 51 | if (child_i_status_ == BT::SUCCESS || child_i_status_ == BT::FAILURE) 52 | { 53 | children_nodes_[0]->set_status(BT::IDLE); 54 | } 55 | 56 | if (child_i_status_ != BT::FAILURE) 57 | { 58 | HaltChildren(1); 59 | } 60 | 61 | set_status(convert(child_i_status_)); 62 | return convert(child_i_status_); 63 | } 64 | } 65 | return BT::EXIT; 66 | } 67 | 68 | BT::ReturnStatus BT::NegationNode::convert(BT::ReturnStatus input) { 69 | switch(input) { 70 | case BT::SUCCESS : return BT::FAILURE; 71 | case BT::FAILURE : return BT::SUCCESS; 72 | default : return input; 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/cpp/condition_example.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | enum Status {RUNNING, SUCCESS, FAILURE}; 20 | 21 | 22 | class BTAction 23 | { 24 | protected: 25 | ros::NodeHandle nh_; 26 | // NodeHandle instance must be created before this line. Otherwise strange errors may occur. 27 | actionlib::SimpleActionServer as_; 28 | std::string action_name_; 29 | // create messages that are used to published feedback/result 30 | behavior_tree_core::BTFeedback feedback_; 31 | behavior_tree_core::BTResult result_; 32 | 33 | public: 34 | explicit BTAction(std::string name) : 35 | as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false), 36 | action_name_(name) 37 | { 38 | // start the action server (action in sense of Actionlib not BT action) 39 | as_.start(); 40 | ROS_INFO("Condition Server Started"); 41 | } 42 | 43 | ~BTAction(void) 44 | { } 45 | void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal) 46 | { 47 | if (true) 48 | { 49 | set_status(SUCCESS); 50 | } 51 | else 52 | { 53 | set_status(FAILURE); 54 | } 55 | } 56 | 57 | // returns the status to the client (Behavior Tree) 58 | void set_status(int status) 59 | { 60 | // Set The feedback and result of BT.action 61 | feedback_.status = status; 62 | result_.status = feedback_.status; 63 | // publish the feedback 64 | as_.publishFeedback(feedback_); 65 | // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE). 66 | as_.setSucceeded(result_); 67 | 68 | switch (status) // Print for convenience 69 | { 70 | case SUCCESS: 71 | ROS_INFO("Condition %s Succeeded", ros::this_node::getName().c_str() ); 72 | break; 73 | case FAILURE: 74 | ROS_INFO("Condition %s Failed", ros::this_node::getName().c_str() ); 75 | break; 76 | default: 77 | break; 78 | } 79 | } 80 | }; 81 | 82 | int main(int argc, char** argv) 83 | { 84 | ros::init(argc, argv, "condition"); 85 | ROS_INFO(" Enum: %d", RUNNING); 86 | ROS_INFO(" condition Ready for Ticks"); 87 | BTAction bt_action(ros::this_node::getName()); 88 | ros::spin(); 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /behavior_tree_core/src/decorator_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::DecoratorNode::DecoratorNode(std::string name) : ControlNode::ControlNode(name) {} 17 | 18 | BT::DecoratorNode::~DecoratorNode() {} 19 | 20 | void BT::DecoratorNode::AddChild(TreeNode* child) 21 | { 22 | // Verify if the decoretor doesn't already has a child 23 | if (children_nodes_.size() > 0) 24 | { 25 | throw BehaviorTreeException("'" + get_name() + " has a child already. A decorator is not capable of handling more than one child node."); 26 | return; 27 | } 28 | 29 | child->set_has_parent(true); 30 | children_nodes_.push_back(child); 31 | children_states_.push_back(BT::IDLE); 32 | } 33 | 34 | BT::ReturnStatus BT::DecoratorNode::Tick() 35 | { 36 | { 37 | // gets the number of children. The number could change if, at runtime, one edits the tree. 38 | N_of_children_ = children_nodes_.size(); 39 | 40 | if (N_of_children_ > 0) 41 | { 42 | if (children_nodes_[0]->get_type() == BT::ACTION_NODE) 43 | { 44 | child_i_status_ = children_nodes_[0]->get_status(); 45 | 46 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 47 | { 48 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[0]->get_name()); 49 | children_nodes_[0]->tick_engine.Tick(); 50 | 51 | // waits for the tick to arrive to the child 52 | do 53 | { 54 | child_i_status_ = children_nodes_[0]->get_status(); 55 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 56 | } 57 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS && child_i_status_ != BT::FAILURE); 58 | } 59 | } 60 | else 61 | { 62 | child_i_status_ = children_nodes_[0]->Tick(); 63 | } 64 | 65 | if (child_i_status_ == BT::SUCCESS || child_i_status_ == BT::FAILURE) 66 | { 67 | children_nodes_[0]->set_status(BT::IDLE); 68 | } 69 | 70 | if (child_i_status_ != BT::FAILURE) 71 | { 72 | HaltChildren(1); 73 | } 74 | 75 | set_status(child_i_status_); 76 | return child_i_status_; 77 | } 78 | } 79 | return BT::EXIT; 80 | } 81 | 82 | int BT::DecoratorNode::DrawType() 83 | { 84 | // Lock acquistion 85 | 86 | return BT::DECORATOR; 87 | } 88 | -------------------------------------------------------------------------------- /behavior_tree_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(behavior_tree_core) 3 | 4 | 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | actionlib 8 | actionlib_msgs 9 | message_generation 10 | roscpp 11 | rospy 12 | std_msgs 13 | roslaunch 14 | genmsg 15 | ) 16 | 17 | 18 | add_action_files( 19 | DIRECTORY action 20 | FILES BT.action 21 | ) 22 | 23 | 24 | generate_messages( 25 | DEPENDENCIES actionlib_msgs std_msgs # Or other packages containing msgs 26 | ) 27 | 28 | 29 | add_definitions(-Wall -lglut -lGL -std=c++0x) 30 | 31 | catkin_package( 32 | CATKIN_DEPENDS actionlib_msgs 33 | INCLUDE_DIRS include 34 | LIBRARIES ${PROJECT_NAME} 35 | ) 36 | 37 | 38 | 39 | ######################################################### 40 | # FIND GLUT 41 | ######################################################### 42 | find_package(GLUT REQUIRED) 43 | include_directories(${GLUT_INCLUDE_DIRS}) 44 | link_directories(${GLUT_LIBRARY_DIRS}) 45 | add_definitions(${GLUT_DEFINITIONS}) 46 | if(NOT GLUT_FOUND) 47 | message(ERROR " GLUT not found!") 48 | endif(NOT GLUT_FOUND) 49 | 50 | ######################################################### 51 | # FIND OPENGL 52 | ######################################################### 53 | find_package(OpenGL REQUIRED) 54 | include_directories(${OpenGL_INCLUDE_DIRS}) 55 | link_directories(${OpenGL_LIBRARY_DIRS}) 56 | add_definitions(${OpenGL_DEFINITIONS}) 57 | if(NOT OPENGL_FOUND) 58 | message(ERROR " OPENGL not found!") 59 | endif(NOT OPENGL_FOUND) 60 | 61 | ######################################################### 62 | # FIND GTest 63 | ######################################################### 64 | find_package(GTest) 65 | include_directories(${GTEST_INCLUDE_DIRS}) 66 | 67 | 68 | INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS} include) 69 | 70 | file(GLOB_RECURSE BTHeadLibrary include/*.h) 71 | 72 | set(BTSrcLibrary 73 | src/action_node.cpp 74 | src/behavior_tree.cpp 75 | src/condition_node.cpp 76 | src/control_node.cpp 77 | src/decorators/negation_node.cpp 78 | #src/decorator_retry_node.cpp 79 | #src/decorator_negation_node.cpp 80 | src/draw.cpp 81 | src/exceptions.cpp 82 | src/leaf_node.cpp 83 | src/tick_engine.cpp 84 | src/parallel_node.cpp 85 | src/fallback_node.cpp 86 | src/sequence_node.cpp 87 | src/decorator_node.cpp 88 | src/fallback_node_with_memory.cpp 89 | src/sequence_node_with_memory.cpp 90 | src/tree_node.cpp 91 | src/actions/action_test_node.cpp 92 | src/conditions/condition_test_node.cpp 93 | src/actions/ros_action.cpp 94 | src/conditions/ros_condition.cpp 95 | src/dot_bt.cpp 96 | ) 97 | 98 | # Compile the core library with name ${PROJECT_NAME}=behavior_tree_core 99 | # You can create executables which target to this library for using BTs 100 | add_library(${PROJECT_NAME} ${BTSrcLibrary} ${BTHeadLibrary}) 101 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY}) 102 | add_dependencies(${PROJECT_NAME} behavior_tree_core_generate_messages_cpp) 103 | 104 | add_executable(tree src/tree.cpp) 105 | target_link_libraries(tree 106 | ${catkin_LIBRARIES} 107 | ${PROJECT_NAME}) 108 | 109 | add_executable(gtest_tree src/gtest/gtest_tree.cpp) 110 | target_link_libraries(gtest_tree 111 | ${catkin_LIBRARIES} 112 | ${PROJECT_NAME} 113 | ${GTEST_LIBRARIES}) 114 | 115 | add_executable(gtest_ros src/gtest/external_ros_nodes_test.cpp) 116 | target_link_libraries(gtest_ros 117 | ${catkin_LIBRARIES} 118 | ${PROJECT_NAME} 119 | ${GTEST_LIBRARIES}) 120 | 121 | 122 | #add_executable(ros_test src/ros_test.cpp ${BTSrcLibrary} ${BTHeadLibrary}) 123 | #target_link_libraries(ros_test ${catkin_LIBRARIES} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY}) 124 | #add_dependencies(ros_test behavior_tree_core_generate_messages_cpp) 125 | 126 | 127 | -------------------------------------------------------------------------------- /behavior_tree_core/src/control_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | BT::ControlNode::ControlNode(std::string name) : TreeNode::TreeNode(name) 19 | { 20 | type_ = BT::CONTROL_NODE; 21 | 22 | // TODO(...) In case it is desired to set to idle remove the ReturnStatus 23 | // type in order to set the member variable 24 | // ReturnStatus child_i_status_ = BT::IDLE; // commented out as unused 25 | } 26 | 27 | BT::ControlNode::~ControlNode() {} 28 | 29 | void BT::ControlNode::AddChild(TreeNode* child) 30 | { 31 | // Checking if the child has a parent already 32 | 33 | if (child->has_parent()) 34 | { 35 | throw BehaviorTreeException("'" + child->get_name() + " has a parent already. Please create different objects for multiple nodes. It makes the tinking/halting precedure easier."); 36 | } 37 | 38 | child->set_has_parent(true); 39 | children_nodes_.push_back(child); 40 | children_states_.push_back(BT::IDLE); 41 | } 42 | 43 | unsigned int BT::ControlNode::GetChildrenNumber() 44 | { 45 | return children_nodes_.size(); 46 | } 47 | 48 | void BT::ControlNode::Halt() 49 | { 50 | DEBUG_STDOUT("HALTING: "<< get_name()); 51 | HaltChildren(0); 52 | set_status(BT::HALTED); 53 | } 54 | 55 | std::vector BT::ControlNode::GetChildren() 56 | { 57 | return children_nodes_; 58 | } 59 | 60 | void BT::ControlNode::ResetColorState() 61 | { 62 | set_color_status(BT::IDLE); 63 | for (unsigned int i = 0; i < children_nodes_.size(); i++) 64 | { 65 | children_nodes_[i]->ResetColorState(); 66 | } 67 | } 68 | 69 | void BT::ControlNode::HaltChildren(int i) 70 | { 71 | for (unsigned int j=i; j < children_nodes_.size(); j++) 72 | { 73 | if (children_nodes_[j]->get_type() == BT::CONDITION_NODE) 74 | { 75 | children_nodes_[i]->ResetColorState(); 76 | } 77 | else 78 | { 79 | if (children_nodes_[j]->get_status() == BT::RUNNING) 80 | { 81 | DEBUG_STDOUT("SENDING HALT TO CHILD " << children_nodes_[j]-> get_name()); 82 | children_nodes_[j]->Halt(); 83 | } 84 | else 85 | { 86 | DEBUG_STDOUT("NO NEED TO HALT " << children_nodes_[j]-> get_name() 87 | << "STATUS" << children_nodes_[j]->get_status()); 88 | } 89 | } 90 | } 91 | } 92 | 93 | int BT::ControlNode::Depth() 94 | { 95 | int depMax = 0; 96 | int dep = 0; 97 | for (unsigned int i = 0; i < children_nodes_.size(); i++) 98 | { 99 | dep = (children_nodes_[i]->Depth()); 100 | if (dep > depMax) 101 | { 102 | depMax = dep; 103 | } 104 | } 105 | return 1 + depMax; 106 | } 107 | -------------------------------------------------------------------------------- /behavior_tree_leaves/example_nodes/cpp/action_example.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | enum Status {RUNNING, SUCCESS, FAILURE}; // BT return status 21 | 22 | 23 | class BTAction 24 | { 25 | protected: 26 | ros::NodeHandle nh_; 27 | // NodeHandle instance must be created before this line. Otherwise strange error may occur. 28 | actionlib::SimpleActionServer as_; 29 | std::string action_name_; 30 | // create messages that are used to published feedback/result 31 | behavior_tree_core::BTFeedback feedback_; // action feedback (SUCCESS, FAILURE) 32 | behavior_tree_core::BTResult result_; // action feedback (same as feedback for us) 33 | 34 | 35 | public: 36 | explicit BTAction(std::string name) : 37 | as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false), 38 | action_name_(name) 39 | { 40 | // Starts the action server 41 | as_.start(); 42 | } 43 | 44 | ~BTAction(void) 45 | {} 46 | 47 | void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal) 48 | { 49 | // publish info to the console for the user 50 | ROS_INFO("Starting Action"); 51 | 52 | // start executing the action 53 | int i = 0; 54 | while (i < 5) 55 | { 56 | // check that preempt has not been requested by the client 57 | if (as_.isPreemptRequested()) 58 | { 59 | ROS_INFO("Action Halted"); 60 | 61 | // set the action state to preempted 62 | as_.setPreempted(); 63 | break; 64 | } 65 | ROS_INFO("Executing Action"); 66 | 67 | ros::Duration(0.5).sleep(); // waiting for 0.5 seconds 68 | i++; 69 | } 70 | 71 | if (i == 5) 72 | { 73 | set_status(SUCCESS); 74 | } 75 | } 76 | 77 | 78 | // returns the status to the client (Behavior Tree) 79 | void set_status(int status) 80 | { 81 | // Set The feedback and result of BT.action 82 | feedback_.status = status; 83 | result_.status = feedback_.status; 84 | // publish the feedback 85 | as_.publishFeedback(feedback_); 86 | // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE). 87 | as_.setSucceeded(result_); 88 | 89 | switch (status) // Print for convenience 90 | { 91 | case SUCCESS: 92 | ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() ); 93 | break; 94 | case FAILURE: 95 | ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() ); 96 | break; 97 | default: 98 | break; 99 | } 100 | } 101 | }; 102 | 103 | 104 | int main(int argc, char** argv) 105 | { 106 | ros::init(argc, argv, "action"); 107 | ROS_INFO(" Enum: %d", RUNNING); 108 | ROS_INFO(" Action Ready for Ticks"); 109 | BTAction bt_action(ros::this_node::getName()); 110 | ros::spin(); 111 | 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /behavior_tree_core/src/sequence_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 11 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 12 | */ 13 | 14 | 15 | #include 16 | #include 17 | 18 | 19 | BT::SequenceNode::SequenceNode(std::string name) : ControlNode::ControlNode(name) {} 20 | 21 | BT::SequenceNode::~SequenceNode() {} 22 | 23 | BT::ReturnStatus BT::SequenceNode::Tick() 24 | { 25 | // gets the number of children. The number could change if, at runtime, one edits the tree. 26 | N_of_children_ = children_nodes_.size(); 27 | 28 | // Routing the ticks according to the sequence node's logic: 29 | 30 | for (unsigned int i = 0; i < N_of_children_; i++) 31 | { 32 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 33 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 34 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 35 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 36 | */ 37 | if (children_nodes_[i]->get_type() == BT::ACTION_NODE) 38 | { 39 | // 1) If the child i is an action, read its state. 40 | child_i_status_ = children_nodes_[i]->get_status(); 41 | 42 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 43 | { 44 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 45 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[i]->get_name()); 46 | children_nodes_[i]->tick_engine.Tick(); 47 | 48 | // waits for the tick to arrive to the child 49 | do 50 | { 51 | child_i_status_ = children_nodes_[i]->get_status(); 52 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 53 | } 54 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 55 | && child_i_status_ != BT::FAILURE); 56 | } 57 | } 58 | else 59 | { 60 | // 2) if it's not an action: 61 | // Send the tick and wait for the response; 62 | child_i_status_ = children_nodes_[i]->Tick(); 63 | } 64 | // Ponderate on which status to send to the parent 65 | if (child_i_status_ != BT::SUCCESS) 66 | { 67 | // If the child status is not success, halt the next children and return the status to your parent. 68 | if (child_i_status_ == BT::FAILURE) 69 | { 70 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned failure. 71 | } 72 | 73 | DEBUG_STDOUT(get_name() << " is HALTING children from " << (i+1)); 74 | HaltChildren(i+1); 75 | set_status(child_i_status_); 76 | return child_i_status_; 77 | } 78 | else 79 | { 80 | // the child returned success. 81 | children_nodes_[i]->set_status(BT::IDLE); 82 | 83 | if (i == N_of_children_ - 1) 84 | { 85 | // If the child status is success, and it is the last child to be ticked, 86 | // then the sequence has succeeded. 87 | set_status(BT::SUCCESS); 88 | return BT::SUCCESS; 89 | } 90 | } 91 | } 92 | return BT::EXIT; 93 | } 94 | 95 | int BT::SequenceNode::DrawType() 96 | { 97 | return BT::SEQUENCE; 98 | } 99 | -------------------------------------------------------------------------------- /behavior_tree_core/src/fallback_node.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::FallbackNode::FallbackNode(std::string name) : ControlNode::ControlNode(name) {} 17 | 18 | BT::FallbackNode::~FallbackNode() {} 19 | 20 | BT::ReturnStatus BT::FallbackNode::Tick() 21 | { 22 | { 23 | // gets the number of children. The number could change if, at runtime, one edits the tree. 24 | N_of_children_ = children_nodes_.size(); 25 | 26 | // Routing the ticks according to the fallback node's logic: 27 | 28 | for (unsigned int i = 0; i < N_of_children_; i++) 29 | { 30 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 31 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 32 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 33 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 34 | */ 35 | if (children_nodes_[i]->get_type() == BT::ACTION_NODE) 36 | { 37 | // 1) If the child i is an action, read its state. 38 | child_i_status_ = children_nodes_[i]->get_status(); 39 | 40 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 41 | { 42 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 43 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[i]->get_name()); 44 | children_nodes_[i]->tick_engine.Tick(); 45 | 46 | // waits for the tick to arrive to the child 47 | do 48 | { 49 | child_i_status_ = children_nodes_[i]->get_status(); 50 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 51 | } 52 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 53 | && child_i_status_ != BT::FAILURE); 54 | } 55 | } 56 | else 57 | { 58 | // 2) if it's not an action: 59 | // Send the tick and wait for the response; 60 | child_i_status_ = children_nodes_[i]->Tick(); 61 | } 62 | // Ponderate on which status to send to the parent 63 | if (child_i_status_ != BT::FAILURE) 64 | { 65 | if (child_i_status_ == BT::SUCCESS) 66 | { 67 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned success. 68 | } 69 | // If the child status is not failure, halt the next children and return the status to your parent. 70 | DEBUG_STDOUT(get_name() << " is HALTING children from " << (i+1)); 71 | HaltChildren(i+1); 72 | set_status(child_i_status_); 73 | return child_i_status_; 74 | } 75 | else 76 | { 77 | // the child returned failure. 78 | children_nodes_[i]->set_status(BT::IDLE); 79 | if (i == N_of_children_ - 1) 80 | { 81 | // If the child status is failure, and it is the last child to be ticked, 82 | // then the sequence has failed. 83 | set_status(BT::FAILURE); 84 | return BT::FAILURE; 85 | } 86 | } 87 | } 88 | } 89 | return BT::EXIT; 90 | } 91 | 92 | int BT::FallbackNode::DrawType() 93 | { 94 | // Lock acquistion 95 | 96 | return BT::SELECTOR; 97 | } 98 | -------------------------------------------------------------------------------- /behavior_tree_core/src/parallel_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 11 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 12 | */ 13 | 14 | 15 | #include 16 | #include 17 | 18 | BT::ParallelNode::ParallelNode(std::string name, int threshold_M) : ControlNode::ControlNode(name) 19 | { 20 | threshold_M_ = threshold_M; 21 | } 22 | 23 | BT::ParallelNode::~ParallelNode() {} 24 | 25 | BT::ReturnStatus BT::ParallelNode::Tick() 26 | { 27 | success_childred_num_ = 0; 28 | failure_childred_num_ = 0; 29 | // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 30 | N_of_children_ = children_nodes_.size(); 31 | 32 | // Routing the tree according to the sequence node's logic: 33 | for (unsigned int i = 0; i < N_of_children_; i++) 34 | { 35 | DEBUG_STDOUT(get_name() << "TICKING " << children_nodes_[i]->get_name()); 36 | 37 | if (children_nodes_[i]->get_type() == BT::ACTION_NODE) 38 | { 39 | // 1) If the child i is an action, read its state. 40 | // Action nodes runs in another parallel, hence you cannot retrieve the status just by executing it. 41 | child_i_status_ = children_nodes_[i]->get_status(); 42 | 43 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 44 | { 45 | // 1.1 If the action status is not running, the sequence node sends a tick to it. 46 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[i]->get_name()); 47 | children_nodes_[i]->tick_engine.Tick(); 48 | 49 | // waits for the tick to arrive to the child 50 | do 51 | { 52 | child_i_status_ = children_nodes_[i]->get_status(); 53 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 54 | } 55 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 56 | && child_i_status_ != BT::FAILURE); 57 | } 58 | } 59 | else 60 | { 61 | child_i_status_ = children_nodes_[i]->Tick(); 62 | } 63 | switch (child_i_status_) 64 | { 65 | case BT::SUCCESS: 66 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned success. 67 | if (++success_childred_num_ == threshold_M_) 68 | { 69 | success_childred_num_ = 0; 70 | failure_childred_num_ = 0; 71 | HaltChildren(0); // halts all running children. The execution is done. 72 | set_status(child_i_status_); 73 | return child_i_status_; 74 | } 75 | break; 76 | case BT::FAILURE: 77 | children_nodes_[i]->set_status(BT::IDLE); // the child goes in idle if it has returned failure. 78 | if (++failure_childred_num_ > N_of_children_- threshold_M_) 79 | { 80 | DEBUG_STDOUT("*******PARALLEL" << get_name() 81 | << " FAILED****** failure_childred_num_:" << failure_childred_num_); 82 | 83 | success_childred_num_ = 0; 84 | failure_childred_num_ = 0; 85 | HaltChildren(0); // halts all running children. The execution is hopeless. 86 | set_status(child_i_status_); 87 | return child_i_status_; 88 | } 89 | break; 90 | case BT::RUNNING: 91 | set_status(child_i_status_); 92 | break; 93 | default: 94 | break; 95 | } 96 | } 97 | return BT::RUNNING; 98 | } 99 | 100 | void BT::ParallelNode::Halt() 101 | { 102 | success_childred_num_ = 0; 103 | failure_childred_num_ = 0; 104 | BT::ControlNode::Halt(); 105 | } 106 | 107 | int BT::ParallelNode::DrawType() 108 | { 109 | return BT::PARALLEL; 110 | } 111 | 112 | 113 | unsigned int BT::ParallelNode::get_threshold_M() 114 | { 115 | return threshold_M_; 116 | } 117 | 118 | void BT::ParallelNode::set_threshold_M(unsigned int threshold_M) 119 | { 120 | threshold_M_ = threshold_M; 121 | } 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /behavior_tree_leaves/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(behavior_tree_leaves) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | genmsg 9 | actionlib_msgs 10 | actionlib 11 | message_generation 12 | roscpp 13 | roslib 14 | rospy 15 | std_msgs 16 | behavior_tree_core 17 | roslaunch 18 | 19 | ) 20 | roslaunch_add_file_check(launch) 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | # System dependencies are found with CMake's conventions 24 | #find_package(Boost REQUIRED COMPONENTS system thread program_options) 25 | 26 | 27 | 28 | # Settings 29 | add_definitions(-Wall -g -O0 -Wno-deprecated -static -Bstatic -std=gnu++0x) 30 | 31 | 32 | ## Uncomment this if the package has a setup.py. This macro ensures 33 | ## modules and global scripts declared therein get installed 34 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 35 | # catkin_python_setup() 36 | 37 | ################################################ 38 | ## Declare ROS messages, services and actions ## 39 | ################################################ 40 | 41 | ## To declare and build messages, services or actions from within this 42 | ## package, follow these steps: 43 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 44 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 45 | ## * In the file package.xml: 46 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 47 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 48 | ## pulled in transitively but can be declared for certainty nonetheless: 49 | ## * add a build_depend tag for "message_generation" 50 | ## * add a run_depend tag for "message_runtime" 51 | ## * In this file (CMakeLists.txt): 52 | ## * add "message_generation" and every package in MSG_DEP_SET to 53 | ## find_package(catkin REQUIRED COMPONENTS ...) 54 | ## * add "message_runtime" and every package in MSG_DEP_SET to 55 | ## catkin_package(CATKIN_DEPENDS ...) 56 | ## * uncomment the add_*_files sections below as needed 57 | ## and list every .msg/.srv/.action file to be processed 58 | ## * uncomment the generate_messages entry below 59 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 60 | 61 | ## Generate messages in the 'msg' folder 62 | # add_message_files( 63 | # FILES 64 | # Message1.msg 65 | # Message2.msg 66 | # ) 67 | 68 | ## Generate services in the 'srv' folder 69 | # add_service_files( 70 | # FILES 71 | # Service1.srv 72 | # Service2.srv 73 | # ) 74 | 75 | ## Generate actions in the 'action' folder 76 | #add_action_files( 77 | # DIRECTORY action 78 | # FILES BT.action 79 | #) 80 | 81 | 82 | 83 | 84 | # add_action_files( 85 | # FILES 86 | # Action1.action 87 | # Action2.action 88 | # ) 89 | 90 | ## Generate added messages and services with any dependencies listed here 91 | 92 | 93 | #generate_messages( 94 | # DEPENDENCIES actionlib_msgs std_msgs behavior_tree_core# Or other packages containing msgs 95 | #) 96 | 97 | 98 | # generate_messages( 99 | # DEPENDENCIES 100 | # actionlib_msgs# std_msgs 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if you package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | #catkin_package( 113 | # INCLUDE_DIRS include 114 | # LIBRARIES learning_actionlib 115 | # CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs 116 | # DEPENDS system_lib 117 | #) 118 | 119 | catkin_package( 120 | CATKIN_DEPENDS actionlib_msgs 121 | ) 122 | 123 | 124 | ########### 125 | ## Build ## 126 | ########### 127 | 128 | ## Specify additional locations of header files 129 | ## Your package locations should be listed before other locations 130 | # include_directories(include) 131 | include_directories( 132 | ${catkin_INCLUDE_DIRS} 133 | ) 134 | 135 | ## Declare a cpp library 136 | # add_library(learning_actionlib 137 | # src/${PROJECT_NAME}/learning_actionlib.cpp 138 | # ) 139 | 140 | set(COMMON_LIB ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 141 | 142 | 143 | add_executable(action_client src/action_client.cpp) 144 | 145 | target_link_libraries( 146 | action_client 147 | ${catkin_LIBRARIES} 148 | ) 149 | 150 | add_dependencies( 151 | action_client 152 | ${behavior_tree_leaves_EXPORTED_TARGETS} 153 | ${PROJECT_NAME}_generate_messages_cpp 154 | behavior_tree_core_generate_messages_cpp 155 | ) 156 | 157 | 158 | add_executable(condition_client src/condition_client.cpp) 159 | 160 | 161 | target_link_libraries( 162 | condition_client 163 | ${catkin_LIBRARIES} 164 | ) 165 | 166 | add_dependencies( 167 | condition_client 168 | ${behavior_tree_leaves_EXPORTED_TARGETS} 169 | ${PROJECT_NAME}_generate_messages_cpp 170 | behavior_tree_core_generate_messages_cpp 171 | 172 | ) 173 | 174 | 175 | 176 | add_executable(action_example example_nodes/cpp/action_example.cpp) 177 | 178 | target_link_libraries( 179 | action_example 180 | ${catkin_LIBRARIES} 181 | ) 182 | 183 | add_dependencies( 184 | action_example 185 | ${behavior_tree_leaves_EXPORTED_TARGETS} 186 | ${PROJECT_NAME}_generate_messages_cpp 187 | behavior_tree_core_generate_messages_cpp 188 | ) 189 | 190 | 191 | add_executable(condition_example example_nodes/cpp/condition_example.cpp) 192 | 193 | target_link_libraries( 194 | condition_example 195 | ${catkin_LIBRARIES} 196 | ) 197 | 198 | add_dependencies( 199 | condition_example 200 | ${behavior_tree_leaves_EXPORTED_TARGETS} 201 | ${PROJECT_NAME}_generate_messages_cpp 202 | behavior_tree_core_generate_messages_cpp 203 | ) 204 | 205 | 206 | 207 | 208 | -------------------------------------------------------------------------------- /behavior_tree_core/src/fallback_node_with_memory.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | BT::FallbackNodeWithMemory::FallbackNodeWithMemory(std::string name) : ControlNode::ControlNode(name) 17 | { 18 | reset_policy_ = BT::ON_SUCCESS_OR_FAILURE; 19 | current_child_idx_ = 0; // initialize the current running child 20 | } 21 | 22 | 23 | BT::FallbackNodeWithMemory::FallbackNodeWithMemory(std::string name, int reset_policy) : ControlNode::ControlNode(name) 24 | { 25 | reset_policy_ = reset_policy; 26 | current_child_idx_ = 0; // initialize the current running child 27 | } 28 | 29 | 30 | BT::FallbackNodeWithMemory::~FallbackNodeWithMemory() {} 31 | 32 | 33 | BT::ReturnStatus BT::FallbackNodeWithMemory::Tick() 34 | { 35 | DEBUG_STDOUT(get_name() << " ticked, memory counter: "<< current_child_idx_); 36 | 37 | // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 38 | N_of_children_ = children_nodes_.size(); 39 | 40 | // Routing the ticks according to the fallback node's (with memory) logic: 41 | while (current_child_idx_ < N_of_children_) 42 | { 43 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 44 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 45 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 46 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 47 | */ 48 | 49 | if (children_nodes_[current_child_idx_]->get_type() == BT::ACTION_NODE) 50 | { 51 | // 1) If the child i is an action, read its state. 52 | // Action nodes runs in another thread, hence you cannot retrieve the status just by executing it. 53 | 54 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 55 | DEBUG_STDOUT(get_name() << " It is an action " << children_nodes_[current_child_idx_]->get_name() 56 | << " with status: " << child_i_status_); 57 | 58 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 59 | { 60 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 61 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[current_child_idx_]->get_name()); 62 | children_nodes_[current_child_idx_]->tick_engine.Tick(); 63 | 64 | // waits for the tick to arrive to the child 65 | do 66 | { 67 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 68 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 69 | } 70 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 71 | && child_i_status_ != BT::FAILURE); 72 | } 73 | } 74 | else 75 | { 76 | // 2) if it's not an action: 77 | // Send the tick and wait for the response; 78 | child_i_status_ = children_nodes_[current_child_idx_]->Tick(); 79 | } 80 | 81 | 82 | if (child_i_status_ == BT::SUCCESS ||child_i_status_ == BT::FAILURE ) 83 | { 84 | // the child goes in idle if it has returned success or failure. 85 | children_nodes_[current_child_idx_]->set_status(BT::IDLE); 86 | } 87 | 88 | if (child_i_status_ != BT::FAILURE) 89 | { 90 | // If the child status is not success, return the status 91 | DEBUG_STDOUT("the status of: " << get_name() << " becomes " << child_i_status_); 92 | if (child_i_status_ == BT::SUCCESS && (reset_policy_ == BT::ON_SUCCESS 93 | || reset_policy_ == BT::ON_SUCCESS_OR_FAILURE)) 94 | { 95 | current_child_idx_ = 0; 96 | } 97 | set_status(child_i_status_); 98 | return child_i_status_; 99 | } 100 | else if (current_child_idx_ != N_of_children_ - 1) 101 | { 102 | // If the child status is failure, continue to the next child 103 | // (if any, hence if(current_child_ != N_of_children_ - 1) ) in the for loop (if any). 104 | current_child_idx_++; 105 | } 106 | else 107 | { 108 | // If it the last child. 109 | if (child_i_status_ == BT::FAILURE) 110 | { 111 | // if it the last child and it has returned failure, reset the memory 112 | current_child_idx_ = 0; 113 | } 114 | set_status(child_i_status_); 115 | return child_i_status_; 116 | } 117 | } 118 | return BT::EXIT; 119 | } 120 | 121 | 122 | int BT::FallbackNodeWithMemory::DrawType() 123 | { 124 | return BT::SELECTORSTAR; 125 | } 126 | 127 | void BT::FallbackNodeWithMemory::Halt() 128 | { 129 | current_child_idx_ = 0; 130 | BT::ControlNode::Halt(); 131 | } 132 | 133 | 134 | -------------------------------------------------------------------------------- /behavior_tree_core/src/sequence_node_with_memory.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 6 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 7 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | * 9 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 11 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 12 | */ 13 | 14 | 15 | #include 16 | #include 17 | 18 | 19 | BT::SequenceNodeWithMemory::SequenceNodeWithMemory(std::string name) : ControlNode::ControlNode(name) 20 | { 21 | reset_policy_ = BT::ON_SUCCESS_OR_FAILURE; 22 | current_child_idx_ = 0; // initialize the current running child 23 | } 24 | 25 | 26 | BT::SequenceNodeWithMemory::SequenceNodeWithMemory(std::string name, int reset_policy) : ControlNode::ControlNode(name) 27 | { 28 | reset_policy_ = reset_policy; 29 | current_child_idx_ = 0; // initialize the current running child 30 | } 31 | 32 | 33 | BT::SequenceNodeWithMemory::~SequenceNodeWithMemory() {} 34 | 35 | 36 | BT::ReturnStatus BT::SequenceNodeWithMemory::Tick() 37 | { 38 | DEBUG_STDOUT(get_name() << " ticked, memory counter: "<< current_child_idx_); 39 | 40 | // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 41 | N_of_children_ = children_nodes_.size(); 42 | 43 | // Routing the ticks according to the sequence node's (with memory) logic: 44 | while (current_child_idx_ < N_of_children_) 45 | { 46 | /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 47 | We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 48 | Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 49 | For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 50 | */ 51 | 52 | if (children_nodes_[current_child_idx_]->get_type() == BT::ACTION_NODE) 53 | { 54 | // 1) If the child i is an action, read its state. 55 | // Action nodes runs in another thread, hence you cannot retrieve the status just by executing it. 56 | 57 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 58 | DEBUG_STDOUT(get_name() << " It is an action " << children_nodes_[current_child_idx_]->get_name() 59 | << " with status: " << child_i_status_); 60 | 61 | if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 62 | { 63 | // 1.1) If the action status is not running, the sequence node sends a tick to it. 64 | DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[current_child_idx_]->get_name()); 65 | children_nodes_[current_child_idx_]->tick_engine.Tick(); 66 | 67 | // waits for the tick to arrive to the child 68 | do 69 | { 70 | child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 71 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 72 | } 73 | while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 74 | && child_i_status_ != BT::FAILURE); 75 | } 76 | } 77 | else 78 | { 79 | // 2) if it's not an action: 80 | // Send the tick and wait for the response; 81 | child_i_status_ = children_nodes_[current_child_idx_]->Tick(); 82 | } 83 | 84 | if (child_i_status_ == BT::SUCCESS ||child_i_status_ == BT::FAILURE ) 85 | { 86 | // the child goes in idle if it has returned success or failure. 87 | children_nodes_[current_child_idx_]->set_status(BT::IDLE); 88 | } 89 | 90 | if (child_i_status_ != BT::SUCCESS) 91 | { 92 | // If the child status is not success, return the status 93 | DEBUG_STDOUT("the status of: " << get_name() << " becomes " << child_i_status_); 94 | if (child_i_status_ == BT::FAILURE && (reset_policy_ == BT::ON_FAILURE 95 | || reset_policy_ == BT::ON_SUCCESS_OR_FAILURE)) 96 | { 97 | current_child_idx_ = 0; 98 | } 99 | set_status(child_i_status_); 100 | return child_i_status_; 101 | } 102 | else if (current_child_idx_ != N_of_children_ - 1) 103 | { 104 | // If the child status is success, continue to the next child 105 | // (if any, hence if(current_child_ != N_of_children_ - 1) ) in the for loop (if any). 106 | current_child_idx_++; 107 | } 108 | else 109 | { 110 | // if it the last child. 111 | if (child_i_status_ == BT::SUCCESS) 112 | { 113 | // if it the last child and it has returned SUCCESS, reset the memory 114 | current_child_idx_ = 0; 115 | } 116 | set_status(child_i_status_); 117 | return child_i_status_; 118 | } 119 | } 120 | return BT::EXIT; 121 | } 122 | 123 | 124 | int BT::SequenceNodeWithMemory::DrawType() 125 | { 126 | return BT::SEQUENCESTAR; 127 | } 128 | 129 | 130 | void BT::SequenceNodeWithMemory::Halt() 131 | { 132 | current_child_idx_ = 0; 133 | BT::ControlNode::Halt(); 134 | } 135 | -------------------------------------------------------------------------------- /behavior_tree_core/include/tree_node.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #ifndef TREE_NODE_H 15 | #define TREE_NODE_H 16 | 17 | 18 | #ifndef _COLORS_ 19 | #define _COLORS_ 20 | 21 | /* FOREGROUND */ 22 | #define RST "\x1B[0m" 23 | #define KRED "\x1B[31m" 24 | #define KGRN "\x1B[32m" 25 | #define KYEL "\x1B[33m" 26 | #define KBLU "\x1B[34m" 27 | #define KMAG "\x1B[35m" 28 | #define KCYN "\x1B[36m" 29 | #define KWHT "\x1B[37m" 30 | 31 | #define FRED(x) KRED x RST 32 | #define FGRN(x) KGRN x RST 33 | #define FYEL(x) KYEL x RST 34 | #define FBLU(x) KBLU x RST 35 | #define FMAG(x) KMAG x RST 36 | #define FCYN(x) KCYN x RST 37 | #define FWHT(x) KWHT x RST 38 | 39 | #define BOLD(x) "\x1B[1m" x RST 40 | #define UNDL(x) "\x1B[4m" x RST 41 | 42 | #endif 43 | 44 | #define DEBUG // uncomment this line if you want to print debug messages 45 | 46 | #ifdef DEBUG 47 | // #define DEBUG_STDERR(x) (std::cerr << (x)) 48 | #define DEBUG_STDOUT(str) do { std::cout << str << std::endl; } while ( false ) // NOLINT(whitespace/braces) 49 | 50 | 51 | #else 52 | #define DEBUG_STDOUT(str) 53 | #endif 54 | 55 | 56 | #include 57 | #include 58 | 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | 67 | #include 68 | #include 69 | 70 | namespace BT 71 | { 72 | // Enumerates the possible types of a node, for drawinf we have do discriminate whoich control node it is: 73 | 74 | enum NodeType {ACTION_NODE, CONDITION_NODE, CONTROL_NODE}; 75 | enum DrawNodeType {PARALLEL, SELECTOR, SEQUENCE, SEQUENCESTAR, SELECTORSTAR, ACTION, CONDITION, DECORATOR}; 76 | // Enumerates the states every node can be in after execution during a particular 77 | // time step: 78 | // - "Success" indicates that the node has completed running during this time step; 79 | // - "Failure" indicates that the node has determined it will not be able to complete 80 | // its task; 81 | // - "Running" indicates that the node has successfully moved forward during this 82 | // time step, but the task is not yet complete; 83 | // - "Idle" indicates that the node hasn't run yet. 84 | // - "Halted" indicates that the node has been halted by its father. 85 | enum ReturnStatus {RUNNING, SUCCESS, FAILURE, IDLE, HALTED, EXIT}; 86 | 87 | // Enumerates the options for when a parallel node is considered to have failed: 88 | // - "FAIL_ON_ONE" indicates that the node will return failure as soon as one of 89 | // its children fails; 90 | // - "FAIL_ON_ALL" indicates that all of the node's children must fail before it 91 | // returns failure. 92 | enum FailurePolicy {FAIL_ON_ONE, FAIL_ON_ALL}; 93 | enum ResetPolity {ON_SUCCESS_OR_FAILURE, ON_SUCCESS, ON_FAILURE}; 94 | 95 | // Enumerates the options for when a parallel node is considered to have succeeded: 96 | // - "SUCCEED_ON_ONE" indicates that the node will return success as soon as one 97 | // of its children succeeds; 98 | // - "BT::SUCCEED_ON_ALL" indicates that all of the node's children must succeed before 99 | // it returns success. 100 | enum SuccessPolicy {SUCCEED_ON_ONE, SUCCEED_ON_ALL}; 101 | 102 | // Abstract base class for Behavior Tree Nodes 103 | class TreeNode 104 | { 105 | private: 106 | // Node name 107 | std::string name_; 108 | bool has_parent_; 109 | 110 | 111 | 112 | 113 | protected: 114 | // The node state that must be treated in a thread-safe way 115 | bool is_state_updated_; 116 | ReturnStatus status_; 117 | ReturnStatus color_status_; 118 | 119 | 120 | std::mutex state_mutex_; 121 | std::mutex color_state_mutex_; 122 | std::condition_variable state_condition_variable_; 123 | // Node type 124 | NodeType type_; 125 | // position and offset for horizontal positioning when drawing 126 | float x_shift_, x_pose_; 127 | 128 | public: 129 | // The thread that will execute the node 130 | std::thread thread_; 131 | 132 | // Node semaphore to simulate the tick 133 | // (and to synchronize fathers and children) 134 | TickEngine tick_engine; 135 | 136 | 137 | 138 | 139 | // The constructor and the distructor 140 | explicit TreeNode(std::string name); 141 | ~TreeNode(); 142 | 143 | // The method that is going to be executed when the node receive a tick 144 | virtual BT::ReturnStatus Tick() = 0; 145 | 146 | // The method used to interrupt the execution of the node 147 | virtual void Halt() = 0; 148 | 149 | // The method that retrive the state of the node 150 | // (conditional waiting and mutual access) 151 | // ReturnStatus GetNodeState(); 152 | void SetNodeState(ReturnStatus new_state); 153 | void set_color_status(ReturnStatus new_color_status); 154 | 155 | // Methods used to access the node state without the 156 | // conditional waiting (only mutual access) 157 | ReturnStatus ReadState(); 158 | ReturnStatus get_color_status(); 159 | virtual int DrawType() = 0; 160 | virtual void ResetColorState() = 0; 161 | virtual int Depth() = 0; 162 | 163 | 164 | // Getters and setters 165 | void set_x_pose(float x_pose); 166 | float get_x_pose(); 167 | 168 | void set_x_shift(float x_shift); 169 | float get_x_shift(); 170 | 171 | 172 | 173 | ReturnStatus get_status(); 174 | void set_status(ReturnStatus new_status); 175 | 176 | 177 | std::string get_name(); 178 | void set_name(std::string new_name); 179 | 180 | NodeType get_type(); 181 | bool has_parent(); 182 | void set_has_parent(bool value); 183 | }; 184 | } // namespace BT 185 | 186 | #endif // TREE_NODE_H 187 | -------------------------------------------------------------------------------- /behavior_tree_core/src/dot_bt.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2017 Iason Sarantopoulos - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a 4 | * copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation 6 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, 7 | * and/or sell copies of the Software, and to permit persons to whom the 8 | * Software is furnished to do so, subject to the following conditions: The 9 | * above copyright notice and this permission notice shall be included in all 10 | * copies or substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 17 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 18 | * DEALINGS IN THE SOFTWARE. 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace BT 28 | { 29 | DotBt::DotBt(TreeNode* root, const std::string& topic, double ros_rate, bool left_right, bool multiple_parents) : 30 | root_(root), 31 | topic_(topic), 32 | loop_rate_(ros_rate), 33 | left_right_(left_right), 34 | multiple_parents_(multiple_parents) 35 | { 36 | dotbt_publisher_ = n_.advertise(topic_, 1); 37 | 38 | ROS_INFO_STREAM("Visualization: Start publishing the tree in topic: " 39 | << topic_ << " with rate: " << ros_rate << " Hz."); 40 | } 41 | 42 | DotBt::~DotBt() {} 43 | 44 | std::string DotBt::defineNodeDot(TreeNode* node, const std::string& alias) 45 | { 46 | std::string output = alias + " "; 47 | 48 | // Find the type of the node and its shape and symbol (label). 49 | switch (node->DrawType()) 50 | { 51 | case SELECTORSTAR: 52 | output += "[label=\"*\n?\" penwidth=\"2\" shape=\"box\""; 53 | break; 54 | case BT::SEQUENCESTAR: 55 | output += "[label=\"*\n-->\" penwidth=\"2\" shape=\"box\""; 56 | break; 57 | case BT::SELECTOR: 58 | output += "[label=\"?\" penwidth=\"2\" shape=\"box\""; 59 | break; 60 | case BT::SEQUENCE: 61 | output += "[label=\"-->\" penwidth=\"2\" shape=\"box\""; 62 | break; 63 | case BT::PARALLEL: 64 | output += "[label=\"-->\n-->\" penwidth=\"2\" shape=\"box\""; 65 | break; 66 | case BT::DECORATOR: 67 | output += "[label=\"D\" penwidth=\"2\" shape=\"diamond\""; 68 | break; 69 | case BT::ACTION: 70 | output += "[label=\"" + node->get_name() + "\" penwidth=\"2\" shape=\"box\" fillcolor=\"palegreen\" style=\"filled\""; 71 | break; 72 | case BT::CONDITION: 73 | output += "[label=\"" + node->get_name() + "\" penwidth=\"2\" shape=\"ellipse\" fillcolor=\"khaki1\" style=\"filled\""; 74 | break; 75 | default: 76 | break; 77 | } 78 | 79 | // Get the current status of the node for the coloring. 80 | switch (node->get_color_status()) 81 | { 82 | case BT::RUNNING: 83 | output += " color=\"black\" ];"; 84 | break; 85 | case BT::SUCCESS: 86 | output += " color=\"green\" ];"; 87 | break; 88 | case BT::FAILURE: 89 | output += " color=\"red\" ];"; 90 | break; 91 | case BT::IDLE: 92 | output += " color=\"gray88\" ];"; 93 | break; 94 | case BT::HALTED: 95 | output += " color=\"orange\" ];"; 96 | break; 97 | default: 98 | output += " color=\"gray88\" ];"; 99 | break; 100 | } 101 | 102 | return output; 103 | } 104 | 105 | void DotBt::produceDot(TreeNode* node, TreeNode* parent, const std::string& parent_alias) 106 | { 107 | // If this node is the root of the tree initialize the directed graph 108 | if (parent == NULL) 109 | { 110 | dot_file_ = "graph behavior_tree {\n"; 111 | if (left_right_) 112 | { 113 | dot_file_ += "rankdir=LR;\n"; 114 | } 115 | 116 | if (!multiple_parents_) 117 | { 118 | aliases_.clear(); 119 | } 120 | } 121 | 122 | // Create an alias for naming the DOT object. 123 | std::string alias = getAlias(node->get_name()); 124 | 125 | // Search if this alias has . In this case change the alias in order to use a 126 | // different visualization instance for this case. 127 | 128 | 129 | if (std::find(aliases_.begin(), aliases_.end(), alias) != aliases_.end()) 130 | { 131 | alias += std::to_string(multiple_alias_solver_++); 132 | } 133 | aliases_.push_back(alias); 134 | 135 | 136 | // Add the definition of this node 137 | dot_file_ += defineNodeDot(node, alias) + "\n"; 138 | 139 | // If the node has a parent, add it as a child of its parent. 140 | if (parent != NULL) 141 | { 142 | dot_file_ += parent_alias + " -- " + alias + ";\n"; 143 | } 144 | 145 | // If this node has children run recursively for each child. 146 | BT::ControlNode* n = dynamic_cast (node); 147 | if (n != NULL) 148 | { 149 | std::vector children = n->GetChildren(); 150 | for (unsigned int i = 0; i < children.size(); i++) 151 | { 152 | 153 | // if (children[i]->has_alias()) 154 | // { 155 | // // cheking if I need to halt the child (has alias) 156 | // for (unsigned int k=0; k < i; k++) 157 | // { 158 | // if (children[k] == children[i] && children[k]->get_status() == BT::HALTED ) 159 | // { 160 | // } 161 | // else 162 | // { 163 | // color_child = false; 164 | // break; 165 | // } 166 | // } 167 | // } 168 | 169 | 170 | 171 | produceDot(children.at(i), node, alias); 172 | } 173 | } 174 | 175 | // In case every recursive calls returns to the root call, close the file. 176 | if (parent == NULL) 177 | { 178 | dot_file_ += "\n}"; 179 | } 180 | } 181 | 182 | std::string DotBt::getAlias(const std::string &name) 183 | { 184 | // Transform name to lower case 185 | std::string out = boost::to_lower_copy(name); 186 | 187 | // If first character is digit add a letter at the beginning 188 | // in order to avoid weird aliases 189 | if (std::isdigit(out.at(0))) 190 | { 191 | out.insert(0, "a"); 192 | } 193 | 194 | 195 | // Replace spaces and dashes with underscore 196 | for (std::string::iterator it = out.begin(); it != out.end(); ++it) 197 | { 198 | if (*it == ' ' || *it == '-') 199 | { 200 | *it = '_'; 201 | } 202 | } 203 | return out; 204 | } 205 | 206 | std::string DotBt::getDotFile() 207 | { 208 | return dot_file_; 209 | } 210 | 211 | void DotBt::publish() 212 | { 213 | std_msgs::String msg; 214 | 215 | // Start the loop for publishing the tree 216 | while (ros::ok()) 217 | { 218 | produceDot(root_); 219 | msg.data = dot_file_; 220 | dotbt_publisher_.publish(msg); 221 | ros::spinOnce(); 222 | loop_rate_.sleep(); 223 | } 224 | } 225 | } // namespace BT 226 | -------------------------------------------------------------------------------- /behavior_tree_core/include/dot_bt.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2017 Iason Sarantopoulos - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a 4 | * copy of this software and associated documentation files (the "Software"), 5 | * to deal in the Software without restriction, including without limitation 6 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, 7 | * and/or sell copies of the Software, and to permit persons to whom the 8 | * Software is furnished to do so, subject to the following conditions: The 9 | * above copyright notice and this permission notice shall be included in all 10 | * copies or substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 17 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 18 | * DEALINGS IN THE SOFTWARE. 19 | */ 20 | 21 | #ifndef DOT_BT_H 22 | #define DOT_BT_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace BT 31 | { 32 | /** 33 | * @brief Provides tools for translate a BT in DOT and publishing it to ROS for 34 | * visualization in RQT. 35 | * 36 | * The class generates code of the DOT graph description language which 37 | * describes the current BT running. It also provide ROS publisher for 38 | * publishing this code in a ROS topic. Then, the user can use the rqt_dot 39 | * plugin in order to visualize in real-time the current tree and the status of 40 | * each node. Regarding multi-parenting this class by default visualize the 41 | * node multiple times under its different parents, in order to have a 42 | * straight-forward visualization. Notice that in the implementation level the 43 | * same node will be ticked. The class also provides the option to visualize 44 | * the nodes with the same name as one node and the nodes with multiple parents 45 | * as without replication (in order to detect visually these undesired 46 | * replications). 47 | * 48 | * Find below an example of use: 49 | * 50 | * @code{.cpp} 51 | * #include 52 | * #include 53 | * 54 | * // ... 55 | * 56 | * // Assume root is a pointer TreeNode* to the root of your tree 57 | * std::string topic = "/my_topic" // The ROS topic to publish the tree 58 | * double rate = 50; // The rate of publishing in Hz 59 | * BT::DotBt dot_bt(root, topic, rate); // Call the constructor 60 | * std::thread t(&BT::DotBt::publish, dot_bt); // A separate thread publishes the tree 61 | * @endcode 62 | */ 63 | class DotBt 64 | { 65 | public: 66 | /** 67 | * @brief The default constructor. 68 | * 69 | * @param root A pointer to the root of the tree. 70 | * @param topic The name of the ROS topic to publish the tree. Defaults to 71 | * "/bt_dotcode". 72 | * @param ros_rate The rate of the publishing in Hz. Defaults to 50Hz. 73 | * @param left_right Set true if a left to right visualization is desired. 74 | * Defaults to true, i.e. top-down visualization 75 | * @param multiple_parents Set true if it is desired to visualize nodes with 76 | * multiple parents (or nodes with the same name) without duplication. It is 77 | * recommended to use the default (false) value for better results. 78 | */ 79 | explicit DotBt(TreeNode* root, 80 | const std::string& topic = "/bt_dotcode", 81 | double ros_rate = 50, 82 | bool left_right = false, 83 | bool multiple_parents = false); 84 | 85 | /** 86 | * @brief An empty destructor. 87 | */ 88 | ~DotBt(); 89 | 90 | /** 91 | * @brief Returns the current DOT code produced for the BT. 92 | * 93 | * @returns The current DOT code 94 | */ 95 | std::string getDotFile(); 96 | 97 | /** 98 | * @brief Publishes the tree for visualization. 99 | * 100 | * This is the main API of the class. It publishes in the given topic with 101 | * the given name the produced DOT code for the current BT running. 102 | * Run it in a separate thread in your BT application. 103 | */ 104 | void publish(); 105 | private: 106 | /** 107 | * @brief Produces DOT code for the tree recursively. 108 | * 109 | * Initially defines the current node calling DotBt::produceDot and then if 110 | * the current node has a parent produces the DOT code for adding this node 111 | * as the child of its parent. If the node has children repeats this process 112 | * recursively. It also checks if the produced alias of the node is already 113 | * existing in order to visualize with a different instance nodes with 114 | * multiple parents or nodes with the same name. 115 | * 116 | * @param node The current node. 117 | * @param parent The parent of the current node. Defaults to NULL for the 118 | * root of the tree. 119 | * @param parent_alias The alias of the parent to be used in the DOT code. 120 | * Defaults to empty string in case this node is the root of the tree. 121 | */ 122 | void produceDot(TreeNode* node, TreeNode* parent = NULL, const std::string& parent_alias = ""); 123 | 124 | /** 125 | * @brief Produces DOT code for the definition of the node. 126 | * 127 | * The current node's alias is used as the DOT object of the node. Then 128 | * checks the type of the node (Action, Sequence etc) and gives the node the 129 | * correct shape and label. Finally it checks the status of the node 130 | * (Running, Idle, Failed etc) in order to give the correct color to each 131 | * node. 132 | * 133 | * @param node A pointer to the node to be defined. 134 | * @param alias The alias of the given node. 135 | * @returns The definition of the Node in DOT 136 | */ 137 | std::string defineNodeDot(TreeNode* node, const std::string& alias); 138 | 139 | /** 140 | * @brief Returns the alias of a node. 141 | * 142 | * In general tranforms a string to lower case and replace the space with 143 | * underscores. E.g. the string "My String" will be returned as "my_string". 144 | * 145 | * @param name The initial string as input 146 | * @returns The final string as output. 147 | */ 148 | std::string getAlias(const std::string &name); 149 | 150 | /** 151 | * @brief Stores the DOT code of the current tree. 152 | */ 153 | std::string dot_file_; 154 | 155 | /** 156 | * @brief A node handle used by the ROS publisher DotBt::dotbt_publisher_. 157 | */ 158 | ros::NodeHandle n_; 159 | 160 | /** 161 | * @brief A ROS publisher for publishing DotBt::dot_file_. 162 | */ 163 | ros::Publisher dotbt_publisher_; 164 | 165 | /** 166 | * @brief The root of the Behavior Tree. 167 | */ 168 | TreeNode* root_; 169 | 170 | /** 171 | * @brief Stores the name of the topic that DotBt::dotbt_publisher_ will publish. 172 | */ 173 | std::string topic_; 174 | 175 | /** 176 | * @brief The rate at which the DotBt::dotbt_publisher_ will publish the tree. 177 | */ 178 | ros::Rate loop_rate_; 179 | 180 | /** 181 | * @brief Stores the aliases of each node of tree. It is used for avoiding 182 | * conflicts due to nodes with the same name or multiple parents. 183 | */ 184 | std::vector aliases_; 185 | 186 | /** 187 | * @brief True for left to right visualization. False for top to down. 188 | */ 189 | bool left_right_; 190 | 191 | /** 192 | * @brief True if you want to visualize nodes with multiple parents without 193 | * duplication. 194 | */ 195 | bool multiple_parents_; 196 | 197 | int multiple_alias_solver_; 198 | }; 199 | } // namespace BT 200 | 201 | #endif // DOT_BT_H 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | NEWS! 2 | ----------- 3 | 4 | 5 | 6 | 7 | Our book **Behavior Trees in Robotics and AI**, published by CRC Press Taylor & Francis, is available for purchase (ebook and hardcover) on the CRC Press Store or Amazon. The Preprint version (**free**) is available here: https://arxiv.org/abs/1709.00084

8 | **Tutorials** available at https://btirai.github.io/ 9 | 10 | 11 | 12 |









13 | 14 | ----------- 15 | 16 | 17 | NOTE: 18 | ------ 19 | The [YARP version](https://github.com/miccol/YARP-Behavior-Trees) of this library has a GUI as the following: 20 | ![alt tag](https://github.com/miccol/YARP-Behavior-Trees/blob/master/YARPBTRun.JPG) 21 | 22 | 23 | ROS-Behavior-Tree ![License MIT](https://img.shields.io/dub/l/vibe-d.svg) 24 | ==== 25 | ![Version](https://img.shields.io/badge/version-v1.3-orange.svg)
26 | A ROS behavior tree library. The leaf nodes (user defined) can be either in `C++` or `python`. Read the user manual for more information. 27 | 28 | REFERENCE 29 | ------------ 30 | Please refer to the following paper when using the library: 31 | 32 | **How Behavior Trees Modularize Hybrid Control Systems and Generalize Sequential Behavior Compositions, the Subsumption Architecture, and Decision Trees.** Michele Colledanchise and Petter Ogren. IEEE Transaction on Robotics 2017. 33 | 34 | bibtex entry: 35 | 36 | `@ARTICLE{TRO17Colledanchise,`
37 | `author={M. Colledanchise and P. Ögren},`
38 | `journal={IEEE Transactions on Robotics},`
39 | `title={{How Behavior Trees Modularize Hybrid Control Systems and Generalize Sequential Behavior Compositions, the Subsumption Architecture, and Decision Trees}},`
40 | `year={2017},`
41 | `volume={33},`
42 | `number={2},`
43 | `pages={372-389},`
44 | `keywords={Computer architecture;Decision trees;High definition video;Robot control;Switches;Behavior trees (BTs);decision trees;finite state machines (FSMs);hybrid dynamical systems (HDSs);modularity;sequential behavior compositions;subsumption architecture}, `
45 | `doi={10.1109/TRO.2016.2633567},`
46 | `ISSN={1552-3098},`
47 | `month={April},}`
48 | 49 | INFO 50 | ------------ 51 | Contains 2 packages: behavior_tree_core and behavior_tree_leaves. 52 | 53 | behavior_tree_core: Contains the core BT source code, including the tree and the leaf nodes. 54 | 55 | behavior_tree_leaves: Contains action and condition specifications for BT leaf nodes **running as external ROS nodes**. 56 | 57 | User manual available in the project folder (BTUserManual.pdf): 58 | 59 | 60 | 61 | 62 | BUILD STATUS 63 | ------------ 64 | 65 | 66 | 67 | 69 | 70 | 71 | 72 | 73 | 74 | 77 | 80 | 83 | 86 | 87 |
68 | HydroIndigoJadeKinetic
Release 75 | 76 | 78 | 79 | 81 | 82 | 84 | 85 |
88 | 89 | DEPENDENCIES 90 | ------------ 91 | 92 | Regarding visualization purposes: 93 | * [Opengl](https://www.opengl.org/) 94 | * [Glut](https://www.opengl.org/resources/libraries/glut/) 95 | * [xdot](https://github.com/jbohren/xdot): For visualizing using DOT language. 96 | * [rqt_dot](https://github.com/jbohren/rqt_dot): For visualizing the tree in RQT with DOT language. 97 | 98 | Regarding unit tests: 99 | * [GTest](https://github.com/google/googletest) 100 | 101 | BT NODES SUPPORT 102 | ---------------- 103 | **Selector:** Selector nodes are used to find and execute the first child that does not fail. A Selector node will return immediately with a status code of success or running when one of its children returns success or running. The children are ticked in order of importance, from `left` to `right`. 104 | 105 | **Sequence:** Sequence nodes are used to find and execute the first child that has not yet succeeded. A sequence node will return immediately with a status code of `failure` or `running` when one of its children returns failure or running. The children are ticked in order, from `left` to `right`. 106 | 107 | **Parallel:** The parallel node ticks its children in parallel and returns success if `M ≤ N` children return success, it returns failure if `N − M + 1` children return failure, and it returns running otherwise. 108 | 109 | **Decorator:** The decorator node manipulates the return status of its child according to the policy defined by the user (e.g. it inverts the success/failure status of the child). In this library the decorators implemented are the two common ones: *Decorator Retry* which retries the execution of a node if this fails; and *Decorator Negation* That inverts the Success/Failure outcome. 110 | 111 | **Action:** An Action node performs an action, and returns Success if the action is completed, Failure if it can not be completed and Running if completion is under way. 112 | 113 | **Condition:** A Condition node determines if a desired condition `c` has been met. Conditions are technically a subset of the Actions, but are given a separate category and graphical symbol to improve readability of the BT and emphasize the fact that they never return running and do not change any internal states/variables of the BT. 114 | 115 | 116 | 117 | 118 | 119 | SETUP 120 | ----------- 121 | **USER MANUAL available inside the repo's folder** 122 | 123 | The first step to use BT++ is to retrieve its source code. You can either download it 124 | here (https://github.com/miccol/ROS-Behavior-Tree) or clone the repository: 125 | 126 | `$ cd /path/to/catkin_ws/src`
127 | `$ git clone https://github.com/miccol/ROS-Behavior-Tree.git`
128 | 129 | Once you have the repository. Compile the library: 130 | 131 | `$ cd /path/to/catkin_ws/`
132 | `$ catkin_make`
133 | 134 | Check the installation by launching an example. 135 | 136 | `$ roslaunch behavior_tree_leaves test_behavior_tree.launch`
137 | 138 | Run `rqt_dot` plugin for the visualization in ROS and put the ROS topic in 139 | which the tree is published. The default topic is `/bt_dotcode`. 140 | 141 | ``` 142 | rosrun rqt_dot rqt_dot 143 | ``` 144 | NOTES 145 | ------- 146 | In case you are puzzled about why a sequence (or fallback) node with 2 or more actions as children never get past the first action, see [this](https://github.com/miccol/ROS-Behavior-Tree/issues/16) discussion. 147 | 148 | LICENSE 149 | ------- 150 | The MIT License (MIT) 151 | 152 | Copyright (c) 2014-2018 Michele Colledanchise 153 | 154 | Permission is hereby granted, free of charge, to any person obtaining a copy 155 | of this software and associated documentation files (the "Software"), to deal 156 | in the Software without restriction, including without limitation the rights 157 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 158 | copies of the Software, and to permit persons to whom the Software is 159 | furnished to do so, subject to the following conditions: 160 | 161 | The above copyright notice and this permission notice shall be included in all 162 | copies or substantial portions of the Software. 163 | 164 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 165 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 166 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 167 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 168 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 169 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 170 | SOFTWARE. 171 | -------------------------------------------------------------------------------- /behavior_tree_core/src/draw.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | const float DEG2RAD = 3.14159/180.0; 22 | 23 | 24 | BT::ControlNode* tree; 25 | bool init = false; 26 | 27 | 28 | void * font_array[3] = {GLUT_BITMAP_8_BY_13, GLUT_BITMAP_8_BY_13, GLUT_BITMAP_8_BY_13}; 29 | void * font = font_array[0]; 30 | float additional_spacing_array[10]; 31 | bool is_number_pressed_array[10]; 32 | unsigned char number_char[4] = {'0', '1', '2', '3'}; 33 | 34 | 35 | float x = 0.0; 36 | float y = 0.4; 37 | float x_offset = 0.01; 38 | float y_offset = 0.15; 39 | float r_color = 1; 40 | float g_color = 1; 41 | float b_color = 1; 42 | GLfloat x_space = 0.06; 43 | 44 | int depth; 45 | 46 | double zoom = 1.0f; 47 | 48 | float fraction = 0.1f; 49 | float zoom_fraction = 0.1f; 50 | 51 | 52 | 53 | 54 | void drawEllipse(float xpos, float ypos, float xradius, float yradius) 55 | { 56 | glBegin(GL_LINE_LOOP); 57 | 58 | for (int i=0; i < 359; i++) 59 | { 60 | // convert degrees into radians 61 | float degInRad = i*DEG2RAD; 62 | glVertex2d(xpos+cos(degInRad)*xradius, ypos + sin(degInRad)*yradius); 63 | } 64 | glEnd(); 65 | } 66 | 67 | void drawString(void * font, const char *string, float x, float y, float z) 68 | { 69 | renderBitmapString(x, y, font, string); 70 | } 71 | 72 | int compute_node_lines(const char *string) 73 | { 74 | const char *c; 75 | int i = 0; 76 | int new_line_num = 1; 77 | glRasterPos2f(x, y); 78 | for (c=string; *c != '\0'; c++) 79 | { 80 | if ((*c == '\n') || ((*c == ' ' && i > 6) || i > 9)) 81 | { 82 | new_line_num++; 83 | i = 0; 84 | continue; 85 | } 86 | i++; 87 | } 88 | return new_line_num; 89 | } 90 | 91 | int compute_max_width(const char *string) 92 | { 93 | const char *current_char; 94 | int current_line_width = 0; 95 | int max_width = 0; 96 | 97 | glRasterPos2f(x, y); 98 | for (current_char = string; *current_char != '\0'; current_char++) 99 | { 100 | if ((*current_char == '\n') || ((*current_char == ' ' && current_line_width > 6) || current_line_width > 9)) 101 | { 102 | if (current_line_width > max_width) 103 | { 104 | max_width = current_line_width; 105 | } 106 | current_line_width = 0; 107 | continue; 108 | } 109 | current_line_width++; 110 | } 111 | 112 | if (max_width == 0) // if the lable fits in a single line 113 | { 114 | max_width = current_line_width; 115 | } 116 | return max_width; 117 | } 118 | 119 | void renderBitmapString(float x, float y, void *font, const char *string) 120 | { 121 | const char *c; 122 | int i = 0; 123 | int new_line_num = 0; 124 | glRasterPos2f(x, y); 125 | for (c=string; *c != '\0'; c++) 126 | { 127 | if ((*c == '\n') || ((*c == ' ' && i > 6) || i > 9)) 128 | { 129 | new_line_num++; 130 | glRasterPos2f(x, y - 0.025*(new_line_num)); 131 | i = 0; 132 | continue; 133 | } 134 | i++; 135 | glutBitmapCharacter(font, *c); 136 | } 137 | } 138 | 139 | 140 | 141 | void draw_node(float x, float y, int node_type, const char *leafName, int status) 142 | { 143 | float NODE_WIDTH = 0.04; 144 | float NODE_HEIGHT = 0.02; 145 | switch (node_type) 146 | { 147 | case BT::SELECTORSTAR: 148 | drawString(font, "?*", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 149 | break; 150 | case BT::SEQUENCESTAR: 151 | drawString(font, ">*", (x + NODE_WIDTH/2 -0.0051), (y - NODE_HEIGHT/2), 0); 152 | break; 153 | case BT::SELECTOR: 154 | drawString(font, "?", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 155 | break; 156 | case BT::SEQUENCE: 157 | drawString(font, ">", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 158 | break; 159 | case BT::PARALLEL: 160 | drawString(font, "=", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 161 | break; 162 | case BT::DECORATOR: 163 | drawString(font, "D", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0); 164 | break; 165 | case BT::ACTION: 166 | { 167 | NODE_HEIGHT = 0.02*(compute_node_lines(leafName)); 168 | std::string st(leafName, 0, 15); 169 | NODE_WIDTH = 0.02*compute_max_width(leafName); 170 | // for (unsigned int i = 0; i < st.size(); i++) 171 | // NODE_WIDTH += 0.01; 172 | } 173 | renderBitmapString((x + 0.015), (y - 0.01), font, leafName); 174 | // glColor3f(0.2, 1.0, 0.2); 175 | break; 176 | case BT::CONDITION: 177 | { 178 | NODE_HEIGHT = 0.02*compute_node_lines(leafName); 179 | std::string st(leafName, 0, 15); 180 | NODE_WIDTH = 0.02*compute_max_width(leafName); 181 | } 182 | renderBitmapString((x + 2*0.015), (y - 0.01), font, leafName); 183 | break; 184 | default: 185 | break; 186 | } 187 | 188 | switch (status) 189 | { 190 | case BT::RUNNING: 191 | glColor3f(0.8, 0.8, 0.8); 192 | break; 193 | case BT::SUCCESS: 194 | glColor3f(0.0, 1.0, 0.0); 195 | break; 196 | case BT::FAILURE: 197 | glColor3f(1.0, 0.0, 0.0); 198 | break; 199 | case BT::IDLE: 200 | glColor3f(0.0, 0.0, 0.0); 201 | break; 202 | case BT::HALTED: 203 | glColor3f(0.0, 0.0, 0.0); 204 | break; 205 | default: 206 | break; 207 | } 208 | 209 | switch (node_type) 210 | { 211 | case BT::CONDITION: 212 | // drawEllipse(x,y,NODE_WIDTH,NODE_HEIGHT); 213 | // drawEllipse(x,y,0.1,0.021); 214 | glBegin(GL_LINE_LOOP); 215 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT - 0.015)); 216 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + 0.02)); 217 | glVertex2f((GLfloat) (x), (GLfloat) (y + 0.02)); 218 | glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT - 0.015)); 219 | glColor3f(0.0, 0.0, 0.0); 220 | glEnd(); 221 | break; 222 | 223 | break; 224 | case BT::ACTION: 225 | 226 | glBegin(GL_LINE_LOOP); 227 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT - 0.015)); 228 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + 0.02)); 229 | glVertex2f((GLfloat) (x), (GLfloat) (y + 0.02)); 230 | glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT - 0.015)); 231 | glColor3f(0.0, 0.0, 0.0); 232 | glEnd(); 233 | break; 234 | 235 | default: 236 | glBegin(GL_LINE_LOOP); 237 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT)); 238 | glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + NODE_HEIGHT)); 239 | glVertex2f((GLfloat) (x), (GLfloat) (y + NODE_HEIGHT)); 240 | glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT)); 241 | glColor3f(0.0, 0.0, 0.0); 242 | glEnd(); 243 | break; 244 | } 245 | } 246 | 247 | // draw the edge connecting one node to the other 248 | void draw_edge(GLfloat parent_x, GLfloat parent_y, 249 | GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size) 250 | { 251 | glLineWidth(1.5); 252 | glColor3f(0.0, 0.0, 0.0); 253 | // GLfloat bottom_spacing = 0.1; // commented-out as unused variable 254 | GLfloat above_spacing = 0.04; 255 | 256 | glBegin(GL_LINES); 257 | glVertex3f(parent_x, parent_y - parent_size, 0); 258 | glVertex3f(parent_x, child_y + child_size + above_spacing, 0); 259 | glEnd(); 260 | 261 | glBegin(GL_LINES); 262 | glVertex3f(parent_x, child_y + child_size + above_spacing, 0); 263 | glVertex3f(child_x, child_y + child_size + above_spacing, 0); 264 | glEnd(); 265 | 266 | glBegin(GL_LINES); 267 | glVertex3f(child_x, child_y + child_size + above_spacing, 0); 268 | glVertex3f(child_x, child_y+child_size, 0); 269 | glEnd(); 270 | } 271 | 272 | // draw the edge connecting one node to the other 273 | void draw_straight_edge(GLfloat parent_x, GLfloat parent_y, 274 | GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size) 275 | { 276 | glLineWidth(1.5); 277 | glColor3f(0.0, 0.0, 0.0); 278 | 279 | glBegin(GL_LINES); 280 | glVertex3f(parent_x, parent_y-parent_size, 0.0); 281 | glVertex3f(child_x, child_y+child_size, 0); 282 | glEnd(); 283 | } 284 | 285 | // Keyboard callback function ( called on keyboard event handling ) 286 | void keyboard(unsigned char key, int x, int y) 287 | { 288 | for (int i = 1; i < 4; i++) 289 | { 290 | if (key == number_char[i]) 291 | { 292 | is_number_pressed_array[i] = true; 293 | } 294 | else 295 | { 296 | is_number_pressed_array[i] = false; 297 | } 298 | } 299 | } 300 | 301 | void keyboard_release(unsigned char key, int x, int y) 302 | { 303 | for (int i = 1; i < 4; i++) 304 | { 305 | if (key == number_char[i]) 306 | { 307 | is_number_pressed_array[i] = false; 308 | } 309 | } 310 | } 311 | 312 | 313 | void drawCircle(float radius) 314 | { 315 | glBegin(GL_LINE_LOOP); 316 | 317 | for (int i=0; i<= 360; i++) 318 | { 319 | float degInRad = i*3.14142/180; 320 | glVertex2f(cos(degInRad)*radius, sin(degInRad)*radius); 321 | } 322 | glEnd(); 323 | } 324 | 325 | 326 | void updateTree(BT::TreeNode* tree, GLfloat x_pos, GLfloat y_pos, GLfloat y_offset, int depth ) 327 | { 328 | BT::ControlNode* d = dynamic_cast (tree); 329 | if (d == NULL) 330 | { 331 | // if it is a leaf node, draw it 332 | draw_node(x_pos , (GLfloat) y_pos, tree->DrawType(), tree->get_name().c_str(), tree->get_color_status()); 333 | } 334 | else 335 | { 336 | // if it is a control flow node, draw it and its children 337 | draw_node((GLfloat) x_pos, (GLfloat) y_pos, tree->DrawType(), 338 | tree->get_name().c_str(), tree->get_color_status()); 339 | std::vector children = d->GetChildren(); 340 | int M = d->GetChildrenNumber(); 341 | std::vector children_x_end; 342 | std::vector children_x_middle_relative; 343 | 344 | GLfloat max_x_end = 0; 345 | // GLfloat max_x_start = 0; // commented out as unused variable 346 | GLfloat current_x_end = 0; 347 | 348 | for (int i = 0; i < M; i++) 349 | { 350 | if (children[i]->DrawType() != BT::ACTION && children[i]->DrawType() != BT::CONDITION) 351 | { 352 | current_x_end = 0.04; 353 | children_x_middle_relative.push_back(0.02); 354 | } 355 | else 356 | { 357 | current_x_end = 0.02*compute_max_width(children[i]->get_name().c_str()); 358 | children_x_middle_relative.push_back(current_x_end/2); 359 | } 360 | 361 | if (i < M-1) 362 | { 363 | max_x_end = max_x_end + current_x_end + x_space + additional_spacing_array[depth]; 364 | } 365 | else 366 | { 367 | max_x_end = max_x_end + current_x_end; 368 | } 369 | children_x_end.push_back(max_x_end); 370 | } 371 | 372 | // GLfloat x_min = 0.0; // commented-out as unused variable 373 | // GLfloat x_max = 0.0; // commented-out as unused variable 374 | GLfloat x_shift = x_pos - max_x_end/2; 375 | // GLfloat x_shift_new = 0; // commented-out as unused variable 376 | 377 | for (int i = 0; i < M; i++) 378 | { 379 | if (i > 0) 380 | { 381 | updateTree(children[i], x_shift + children_x_end.at(i - 1) , y_pos - y_offset , y_offset, depth + 1); 382 | draw_edge(x_pos + 0.015, y_pos, 0.02, 383 | x_shift + children_x_end.at(i-1) + children_x_middle_relative.at(i), 384 | y_pos - y_offset, 0.02); 385 | } 386 | else 387 | { 388 | updateTree(children[i], x_shift , y_pos - y_offset , y_offset, depth + 1); 389 | draw_edge(x_pos + 0.015, y_pos, 0.02, 390 | x_shift + children_x_middle_relative.at(i), y_pos - y_offset, 0.02); 391 | } 392 | } 393 | } 394 | } 395 | 396 | 397 | 398 | 399 | 400 | void display() 401 | { 402 | glClearColor(r_color, g_color, b_color, 0.1); // clear the draw buffer . 403 | glClear(GL_COLOR_BUFFER_BIT); // Erase everything 404 | updateTree(tree, x , y, y_offset, 1); 405 | glutSwapBuffers(); 406 | glutPostRedisplay(); 407 | } 408 | 409 | 410 | void processSpecialKeys(int key, int xx, int yy) 411 | { 412 | switch (key) 413 | { 414 | case GLUT_KEY_UP : 415 | y += fraction; 416 | break; 417 | case GLUT_KEY_DOWN : 418 | y -= fraction; 419 | break; 420 | case GLUT_KEY_LEFT: 421 | x -= fraction; 422 | break; 423 | case GLUT_KEY_RIGHT: 424 | x += fraction; 425 | break; 426 | case GLUT_KEY_PAGE_UP: 427 | for (int i = 1; i < 10; i++) 428 | { 429 | if (is_number_pressed_array[i]) 430 | { 431 | additional_spacing_array[i] += fraction; 432 | } 433 | } 434 | break; 435 | case GLUT_KEY_PAGE_DOWN: 436 | for (int i = 1; i < 10; i++) 437 | { 438 | if (is_number_pressed_array[i] && additional_spacing_array[i] >= 0 ) 439 | { 440 | additional_spacing_array[i] -= fraction; 441 | } 442 | } 443 | break; 444 | case GLUT_KEY_F1: 445 | if (r_color < 1) r_color += fraction; 446 | break; 447 | case GLUT_KEY_F2: 448 | if (r_color > 0) r_color -= fraction; 449 | break; 450 | case GLUT_KEY_F3: 451 | if (g_color < 1) g_color += fraction; 452 | break; 453 | case GLUT_KEY_F4: 454 | if (g_color > 0) g_color -= fraction; 455 | break; 456 | case GLUT_KEY_F5: 457 | if (b_color < 1) b_color += fraction; 458 | break; 459 | case GLUT_KEY_F6: 460 | if (b_color > 0) b_color -= fraction; 461 | break; 462 | case GLUT_KEY_HOME: 463 | if (zoom < 1.0f) 464 | { 465 | glScalef(1.0f + zoom_fraction, 1.0f + zoom_fraction, 1.0f); 466 | zoom += zoom_fraction; 467 | } 468 | else 469 | { 470 | glScalef(1.0f, 1.0f, 1.0f); 471 | } 472 | break; 473 | case GLUT_KEY_END: 474 | glScalef(1.0f - zoom_fraction, 1.0f - zoom_fraction, 1.0f); 475 | zoom -= zoom_fraction; 476 | break; 477 | } 478 | } 479 | 480 | void drawTree(BT::ControlNode* tree_) 481 | { 482 | /***************************BT VISUALIZATION****************************/ 483 | int argc = 1; 484 | char *argv[1] = {const_cast("")}; 485 | 486 | if (!init) 487 | { 488 | XInitThreads(); 489 | glutInit(&argc, argv); 490 | init = true; 491 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_MULTISAMPLE); // Antialiasing 492 | glEnable(GL_MULTISAMPLE); 493 | } 494 | tree = tree_; 495 | depth = tree->Depth(); 496 | 497 | glutInitWindowSize(1024, 860); 498 | 499 | glutCreateWindow("Behavior Tree"); // Create a window 500 | 501 | glClearColor(0, 0.71, 0.00, 0.1); 502 | glutDisplayFunc(display); // Register display callback 503 | 504 | glutKeyboardFunc(keyboard); // Register keyboard presscallback 505 | glutKeyboardUpFunc(keyboard_release); // Register keyboard release callback 506 | 507 | glutSpecialFunc(processSpecialKeys); // Register keyboard arrow callback 508 | 509 | glutMainLoop(); // Enter main event loop 510 | 511 | /***************************ENDOF BT VISUALIZATION ****************************/ 512 | } 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | -------------------------------------------------------------------------------- /behavior_tree_core/src/gtest/gtest_tree.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 10 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 11 | */ 12 | 13 | 14 | #include 15 | #include 16 | 17 | 18 | 19 | struct SimpleSequenceTest : testing::Test 20 | { 21 | BT:: SequenceNode* root; 22 | BT::ActionTestNode* action; 23 | BT::ConditionTestNode* condition; 24 | SimpleSequenceTest() 25 | { 26 | action = new BT::ActionTestNode("action"); 27 | condition = new BT::ConditionTestNode("condition"); 28 | 29 | root = new BT::SequenceNode("seq1"); 30 | 31 | root->AddChild(condition); 32 | root->AddChild(action); 33 | } 34 | }; 35 | 36 | 37 | struct ComplexSequenceTest : testing::Test 38 | { 39 | BT:: SequenceNode* root; 40 | BT::ActionTestNode* action_1; 41 | BT::ConditionTestNode* condition_1; 42 | BT::ConditionTestNode* condition_2; 43 | 44 | BT:: SequenceNode* seq_conditions; 45 | 46 | ComplexSequenceTest() 47 | { 48 | action_1 = new BT::ActionTestNode("action 1"); 49 | 50 | condition_1 = new BT::ConditionTestNode("condition 1"); 51 | condition_2 = new BT::ConditionTestNode("condition 2"); 52 | seq_conditions = new BT::SequenceNode("sequence_conditions"); 53 | 54 | seq_conditions->AddChild(condition_1); 55 | seq_conditions->AddChild(condition_2); 56 | 57 | root = new BT::SequenceNode("root"); 58 | root->AddChild(seq_conditions); 59 | root->AddChild(action_1); 60 | } 61 | }; 62 | 63 | 64 | struct ComplexSequence2ActionsTest : testing::Test 65 | { 66 | BT:: SequenceNode* root; 67 | BT::ActionTestNode* action_1; 68 | BT::ActionTestNode* action_2; 69 | 70 | BT::ConditionTestNode* condition_1; 71 | BT::ConditionTestNode* condition_2; 72 | 73 | BT:: SequenceNode* seq_1; 74 | BT:: SequenceNode* seq_2; 75 | 76 | ComplexSequence2ActionsTest() 77 | { 78 | action_1 = new BT::ActionTestNode("action 1"); 79 | action_2 = new BT::ActionTestNode("action 2"); 80 | seq_1 = new BT::SequenceNode("sequence_1"); 81 | seq_2 = new BT::SequenceNode("sequence_c2"); 82 | 83 | condition_1 = new BT::ConditionTestNode("condition 1"); 84 | condition_2 = new BT::ConditionTestNode("condition 2"); 85 | 86 | seq_1->AddChild(condition_1); 87 | seq_1->AddChild(action_1); 88 | 89 | seq_2->AddChild(condition_2); 90 | seq_2->AddChild(action_2); 91 | 92 | 93 | root = new BT::SequenceNode("root"); 94 | root->AddChild(seq_1); 95 | root->AddChild(seq_2); 96 | } 97 | }; 98 | 99 | 100 | struct SimpleFallbackTest : testing::Test 101 | { 102 | BT:: FallbackNode* root; 103 | BT::ActionTestNode* action; 104 | BT::ConditionTestNode* condition; 105 | SimpleFallbackTest() 106 | { 107 | action = new BT::ActionTestNode("action"); 108 | condition = new BT::ConditionTestNode("condition"); 109 | 110 | root = new BT::FallbackNode("seq1"); 111 | 112 | root->AddChild(condition); 113 | root->AddChild(action); 114 | } 115 | }; 116 | 117 | 118 | struct ComplexFallbackTest : testing::Test 119 | { 120 | BT:: FallbackNode* root; 121 | BT::ActionTestNode* action_1; 122 | BT::ConditionTestNode* condition_1; 123 | BT::ConditionTestNode* condition_2; 124 | 125 | BT:: FallbackNode* sel_conditions; 126 | 127 | ComplexFallbackTest() 128 | { 129 | action_1 = new BT::ActionTestNode("action 1"); 130 | condition_1 = new BT::ConditionTestNode("condition 1"); 131 | condition_2 = new BT::ConditionTestNode("condition 2"); 132 | sel_conditions = new BT::FallbackNode("fallback_conditions"); 133 | 134 | sel_conditions->AddChild(condition_1); 135 | sel_conditions->AddChild(condition_2); 136 | 137 | root = new BT::FallbackNode("root"); 138 | root->AddChild(sel_conditions); 139 | root->AddChild(action_1); 140 | } 141 | }; 142 | 143 | 144 | 145 | 146 | struct BehaviorTreeTest : testing::Test 147 | { 148 | BT:: SequenceNode* root; 149 | BT::ActionTestNode* action_1; 150 | BT::ConditionTestNode* condition_1; 151 | BT::ConditionTestNode* condition_2; 152 | 153 | BT:: FallbackNode* sel_conditions; 154 | 155 | BehaviorTreeTest() 156 | { 157 | action_1 = new BT::ActionTestNode("action 1"); 158 | condition_1 = new BT::ConditionTestNode("condition 1"); 159 | condition_2 = new BT::ConditionTestNode("condition 2"); 160 | sel_conditions = new BT::FallbackNode("fallback_conditions"); 161 | 162 | sel_conditions->AddChild(condition_1); 163 | sel_conditions->AddChild(condition_2); 164 | 165 | root = new BT::SequenceNode("root"); 166 | root->AddChild(sel_conditions); 167 | root->AddChild(action_1); 168 | } 169 | }; 170 | 171 | 172 | 173 | 174 | struct SimpleSequenceWithMemoryTest : testing::Test 175 | { 176 | BT:: SequenceNodeWithMemory* root; 177 | BT::ActionTestNode* action; 178 | BT::ConditionTestNode* condition; 179 | SimpleSequenceWithMemoryTest() 180 | { 181 | action = new BT::ActionTestNode("action"); 182 | condition = new BT::ConditionTestNode("condition"); 183 | 184 | root = new BT::SequenceNodeWithMemory("seq1"); 185 | 186 | root->AddChild(condition); 187 | root->AddChild(action); 188 | } 189 | }; 190 | 191 | struct ComplexSequenceWithMemoryTest : testing::Test 192 | { 193 | BT:: SequenceNodeWithMemory* root; 194 | 195 | BT::ActionTestNode* action_1; 196 | BT::ActionTestNode* action_2; 197 | 198 | BT::ConditionTestNode* condition_1; 199 | BT::ConditionTestNode* condition_2; 200 | 201 | BT:: SequenceNodeWithMemory* seq_conditions; 202 | BT:: SequenceNodeWithMemory* seq_actions; 203 | 204 | ComplexSequenceWithMemoryTest() 205 | { 206 | action_1 = new BT::ActionTestNode("action 1"); 207 | action_2 = new BT::ActionTestNode("action 2"); 208 | 209 | 210 | condition_1 = new BT::ConditionTestNode("condition 1"); 211 | condition_2 = new BT::ConditionTestNode("condition 2"); 212 | 213 | seq_conditions = new BT::SequenceNodeWithMemory("sequence_conditions"); 214 | seq_actions = new BT::SequenceNodeWithMemory("sequence_actions"); 215 | 216 | seq_actions->AddChild(action_1); 217 | seq_actions->AddChild(action_2); 218 | 219 | seq_conditions->AddChild(condition_1); 220 | seq_conditions->AddChild(condition_2); 221 | 222 | root = new BT::SequenceNodeWithMemory("root"); 223 | root->AddChild(seq_conditions); 224 | root->AddChild(seq_actions); 225 | } 226 | }; 227 | 228 | struct SimpleFallbackWithMemoryTest : testing::Test 229 | { 230 | BT::FallbackNodeWithMemory* root; 231 | BT::ActionTestNode* action; 232 | BT::ConditionTestNode* condition; 233 | SimpleFallbackWithMemoryTest() 234 | { 235 | action = new BT::ActionTestNode("action"); 236 | condition = new BT::ConditionTestNode("condition"); 237 | 238 | root = new BT::FallbackNodeWithMemory("seq1"); 239 | 240 | root->AddChild(condition); 241 | root->AddChild(action); 242 | } 243 | }; 244 | 245 | struct ComplexFallbackWithMemoryTest : testing::Test 246 | { 247 | BT:: FallbackNodeWithMemory* root; 248 | 249 | BT::ActionTestNode* action_1; 250 | BT::ActionTestNode* action_2; 251 | 252 | BT::ConditionTestNode* condition_1; 253 | BT::ConditionTestNode* condition_2; 254 | 255 | BT:: FallbackNodeWithMemory* fal_conditions; 256 | BT:: FallbackNodeWithMemory* fal_actions; 257 | 258 | ComplexFallbackWithMemoryTest() 259 | { 260 | action_1 = new BT::ActionTestNode("action 1"); 261 | action_2 = new BT::ActionTestNode("action 2"); 262 | condition_1 = new BT::ConditionTestNode("condition 1"); 263 | condition_2 = new BT::ConditionTestNode("condition 2"); 264 | 265 | fal_conditions = new BT::FallbackNodeWithMemory("fallback_conditions"); 266 | fal_actions = new BT::FallbackNodeWithMemory("fallback_actions"); 267 | 268 | fal_actions->AddChild(action_1); 269 | fal_actions->AddChild(action_2); 270 | 271 | fal_conditions->AddChild(condition_1); 272 | fal_conditions->AddChild(condition_2); 273 | 274 | root = new BT::FallbackNodeWithMemory("root"); 275 | root->AddChild(fal_conditions); 276 | root->AddChild(fal_actions); 277 | } 278 | }; 279 | 280 | 281 | struct SimpleParallelTest : testing::Test 282 | { 283 | BT::ParallelNode* root; 284 | BT::ActionTestNode* action_1; 285 | BT::ConditionTestNode* condition_1; 286 | 287 | BT::ActionTestNode* action_2; 288 | BT::ConditionTestNode* condition_2; 289 | 290 | SimpleParallelTest() 291 | { 292 | action_1 = new BT::ActionTestNode("action 1"); 293 | condition_1 = new BT::ConditionTestNode("condition 1"); 294 | 295 | 296 | action_2 = new BT::ActionTestNode("action 2"); 297 | condition_2 = new BT::ConditionTestNode("condition 2"); 298 | 299 | root = new BT::ParallelNode("par", 4); 300 | 301 | root->AddChild(condition_1); 302 | root->AddChild(action_1); 303 | root->AddChild(condition_2); 304 | root->AddChild(action_2); 305 | } 306 | }; 307 | 308 | 309 | struct ComplexParallelTest : testing::Test 310 | { 311 | BT::ParallelNode* root; 312 | BT::ParallelNode* parallel_1; 313 | BT::ParallelNode* parallel_2; 314 | 315 | BT::ActionTestNode* action_1; 316 | BT::ConditionTestNode* condition_1; 317 | 318 | BT::ActionTestNode* action_2; 319 | BT::ConditionTestNode* condition_2; 320 | 321 | BT::ActionTestNode* action_3; 322 | BT::ConditionTestNode* condition_3; 323 | 324 | ComplexParallelTest() 325 | { 326 | action_1 = new BT::ActionTestNode("action 1"); 327 | condition_1 = new BT::ConditionTestNode("condition 1"); 328 | 329 | 330 | action_2 = new BT::ActionTestNode("action 2"); 331 | condition_2 = new BT::ConditionTestNode("condition 2"); 332 | 333 | 334 | action_3 = new BT::ActionTestNode("action 3"); 335 | condition_3 = new BT::ConditionTestNode("condition 3"); 336 | 337 | root = new BT::ParallelNode("root", 2); 338 | parallel_1 = new BT::ParallelNode("par1", 3); 339 | parallel_2 = new BT::ParallelNode("par2", 1); 340 | 341 | parallel_1->AddChild(condition_1); 342 | parallel_1->AddChild(action_1); 343 | parallel_1->AddChild(condition_2); 344 | parallel_1->AddChild(action_2); 345 | 346 | parallel_2->AddChild(condition_3); 347 | parallel_2->AddChild(action_3); 348 | 349 | root->AddChild(parallel_1); 350 | root->AddChild(parallel_2); 351 | } 352 | }; 353 | 354 | 355 | 356 | /****************TESTS START HERE***************************/ 357 | 358 | 359 | 360 | TEST_F(SimpleSequenceTest, ConditionTrue) 361 | { 362 | std::cout << "Ticking the root node !" << std::endl << std::endl; 363 | // Ticking the root node 364 | BT::ReturnStatus state = root->Tick(); 365 | 366 | ASSERT_EQ(BT::RUNNING, action->get_status()); 367 | ASSERT_EQ(BT::RUNNING, state); 368 | root->Halt(); 369 | } 370 | 371 | 372 | TEST_F(SimpleSequenceTest, ConditionTurnToFalse) 373 | { 374 | BT::ReturnStatus state = root->Tick(); 375 | condition->set_boolean_value(false); 376 | 377 | state = root->Tick(); 378 | ASSERT_EQ(BT::FAILURE, state); 379 | ASSERT_EQ(BT::HALTED, action->get_status()); 380 | root->Halt(); 381 | } 382 | 383 | 384 | TEST_F(ComplexSequenceTest, ComplexSequenceConditionsTrue) 385 | { 386 | BT::ReturnStatus state = root->Tick(); 387 | 388 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 389 | ASSERT_EQ(BT::RUNNING, state); 390 | root->Halt(); 391 | } 392 | 393 | 394 | TEST_F(ComplexSequence2ActionsTest, ConditionsTrue) 395 | { 396 | BT::ReturnStatus state = root->Tick(); 397 | 398 | state = root->Tick(); 399 | 400 | std::this_thread::sleep_for(std::chrono::seconds(5)); 401 | state = root->Tick(); 402 | state = root->Tick(); 403 | 404 | 405 | ASSERT_EQ(BT::RUNNING, state); 406 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 407 | ASSERT_EQ(BT::RUNNING, seq_1->get_status()); 408 | ASSERT_EQ(BT::HALTED, seq_2->get_status()); 409 | ASSERT_EQ(BT::HALTED, action_2->get_status()); 410 | 411 | root->Halt(); 412 | } 413 | 414 | TEST_F(ComplexSequenceTest, ComplexSequenceConditions1ToFalse) 415 | { 416 | BT::ReturnStatus state = root->Tick(); 417 | 418 | condition_1->set_boolean_value(false); 419 | 420 | state = root->Tick(); 421 | 422 | ASSERT_EQ(BT::FAILURE, state); 423 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 424 | root->Halt(); 425 | } 426 | 427 | TEST_F(ComplexSequenceTest, ComplexSequenceConditions2ToFalse) 428 | { 429 | BT::ReturnStatus state = root->Tick(); 430 | 431 | condition_2->set_boolean_value(false); 432 | 433 | state = root->Tick(); 434 | 435 | ASSERT_EQ(BT::FAILURE, state); 436 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 437 | root->Halt(); 438 | } 439 | 440 | 441 | 442 | TEST_F(SimpleFallbackTest, ConditionTrue) 443 | { 444 | std::cout << "Ticking the root node !" << std::endl << std::endl; 445 | // Ticking the root node 446 | condition->set_boolean_value(true); 447 | BT::ReturnStatus state = root->Tick(); 448 | 449 | ASSERT_EQ(BT::IDLE, action->get_status()); 450 | ASSERT_EQ(BT::SUCCESS, state); 451 | root->Halt(); 452 | } 453 | 454 | 455 | TEST_F(SimpleFallbackTest, ConditionToFalse) 456 | { 457 | condition->set_boolean_value(false); 458 | 459 | BT::ReturnStatus state = root->Tick(); 460 | condition->set_boolean_value(true); 461 | 462 | 463 | state = root->Tick(); 464 | 465 | ASSERT_EQ(BT::SUCCESS, state); 466 | ASSERT_EQ(BT::HALTED, action->get_status()); 467 | root->Halt(); 468 | } 469 | 470 | 471 | TEST_F(ComplexFallbackTest, Condition1ToTrue) 472 | { 473 | condition_1->set_boolean_value(false); 474 | condition_2->set_boolean_value(false); 475 | 476 | BT::ReturnStatus state = root->Tick(); 477 | 478 | condition_1->set_boolean_value(true); 479 | 480 | state = root->Tick(); 481 | 482 | ASSERT_EQ(BT::SUCCESS, state); 483 | 484 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 485 | root->Halt(); 486 | } 487 | 488 | TEST_F(ComplexFallbackTest, Condition2ToTrue) 489 | { 490 | condition_1->set_boolean_value(false); 491 | condition_2->set_boolean_value(false); 492 | 493 | BT::ReturnStatus state = root->Tick(); 494 | 495 | condition_2->set_boolean_value(true); 496 | 497 | state = root->Tick(); 498 | 499 | ASSERT_EQ(BT::SUCCESS, state); 500 | ASSERT_EQ(BT::HALTED, action_1->get_status()); 501 | root->Halt(); 502 | } 503 | 504 | 505 | 506 | TEST_F(BehaviorTreeTest, Condition1ToFalseCondition2True) 507 | { 508 | condition_1->set_boolean_value(false); 509 | condition_2->set_boolean_value(true); 510 | 511 | BT::ReturnStatus state = root->Tick(); 512 | 513 | ASSERT_EQ(BT::RUNNING, state); 514 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 515 | 516 | root->Halt(); 517 | } 518 | 519 | TEST_F(BehaviorTreeTest, Condition2ToFalseCondition1True) 520 | { 521 | condition_2->set_boolean_value(false); 522 | condition_1->set_boolean_value(true); 523 | 524 | BT::ReturnStatus state = root->Tick(); 525 | 526 | ASSERT_EQ(BT::RUNNING, state); 527 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 528 | 529 | root->Halt(); 530 | } 531 | 532 | 533 | TEST_F(SimpleSequenceWithMemoryTest, ConditionTrue) 534 | { 535 | std::cout << "Ticking the root node !" << std::endl << std::endl; 536 | // Ticking the root node 537 | BT::ReturnStatus state = root->Tick(); 538 | std::this_thread::sleep_for(std::chrono::seconds(1)); 539 | 540 | ASSERT_EQ(BT::RUNNING, action->get_status()); 541 | ASSERT_EQ(BT::RUNNING, state); 542 | root->Halt(); 543 | } 544 | 545 | 546 | TEST_F(SimpleSequenceWithMemoryTest, ConditionTurnToFalse) 547 | { 548 | BT::ReturnStatus state = root->Tick(); 549 | 550 | condition->set_boolean_value(false); 551 | 552 | state = root->Tick(); 553 | 554 | ASSERT_EQ(BT::RUNNING, state); 555 | ASSERT_EQ(BT::RUNNING, action->get_status()); 556 | 557 | root->Halt(); 558 | } 559 | 560 | 561 | TEST_F(ComplexSequenceWithMemoryTest, ConditionsTrue) 562 | { 563 | BT::ReturnStatus state = root->Tick(); 564 | 565 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 566 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 567 | ASSERT_EQ(BT::RUNNING, state); 568 | 569 | root->Halt(); 570 | } 571 | 572 | 573 | TEST_F(ComplexSequenceWithMemoryTest, Conditions1ToFalse) 574 | { 575 | BT::ReturnStatus state = root->Tick(); 576 | 577 | condition_1->set_boolean_value(false); 578 | 579 | state = root->Tick(); 580 | 581 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 582 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 583 | ASSERT_EQ(BT::RUNNING, state); 584 | root->Halt(); 585 | } 586 | 587 | TEST_F(ComplexSequenceWithMemoryTest, Conditions2ToFalse) 588 | { 589 | BT::ReturnStatus state = root->Tick(); 590 | 591 | condition_2->set_boolean_value(false); 592 | 593 | state = root->Tick(); 594 | 595 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 596 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 597 | ASSERT_EQ(BT::RUNNING, state); 598 | 599 | root->Halt(); 600 | } 601 | 602 | TEST_F(ComplexSequenceWithMemoryTest, Action1Done) 603 | { 604 | root->Tick(); 605 | 606 | condition_2->set_boolean_value(false); 607 | 608 | root->Tick(); 609 | std::this_thread::sleep_for(std::chrono::seconds(10)); 610 | root->Tick(); 611 | 612 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 613 | 614 | root->Halt(); 615 | } 616 | 617 | TEST_F(SimpleFallbackWithMemoryTest, ConditionFalse) 618 | { 619 | std::cout << "Ticking the root node !" << std::endl << std::endl; 620 | // Ticking the root node 621 | condition->set_boolean_value(false); 622 | BT::ReturnStatus state = root->Tick(); 623 | std::this_thread::sleep_for(std::chrono::seconds(1)); 624 | 625 | ASSERT_EQ(BT::RUNNING, action->get_status()); 626 | ASSERT_EQ(BT::RUNNING, state); 627 | 628 | root->Halt(); 629 | } 630 | 631 | 632 | TEST_F(SimpleFallbackWithMemoryTest, ConditionTurnToTrue) 633 | { 634 | condition->set_boolean_value(false); 635 | 636 | BT::ReturnStatus state = root->Tick(); 637 | condition->set_boolean_value(true); 638 | 639 | state = root->Tick(); 640 | 641 | ASSERT_EQ(BT::RUNNING, state); 642 | ASSERT_EQ(BT::RUNNING, action->get_status()); 643 | 644 | root->Halt(); 645 | } 646 | 647 | 648 | 649 | TEST_F(ComplexFallbackWithMemoryTest, ConditionsTrue) 650 | { 651 | BT::ReturnStatus state = root->Tick(); 652 | 653 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 654 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 655 | ASSERT_EQ(BT::SUCCESS, state); 656 | 657 | root->Halt(); 658 | } 659 | 660 | TEST_F(ComplexFallbackWithMemoryTest, Condition1False) 661 | { 662 | condition_1->set_boolean_value(false); 663 | BT::ReturnStatus state = root->Tick(); 664 | 665 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 666 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 667 | ASSERT_EQ(BT::SUCCESS, state); 668 | 669 | root->Halt(); 670 | } 671 | 672 | 673 | TEST_F(ComplexFallbackWithMemoryTest, ConditionsFalse) 674 | { 675 | condition_1->set_boolean_value(false); 676 | condition_2->set_boolean_value(false); 677 | BT::ReturnStatus state = root->Tick(); 678 | 679 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 680 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 681 | ASSERT_EQ(BT::RUNNING, state); 682 | 683 | root->Halt(); 684 | } 685 | 686 | 687 | 688 | 689 | TEST_F(ComplexFallbackWithMemoryTest, Conditions1ToTrue) 690 | { 691 | condition_1->set_boolean_value(false); 692 | condition_2->set_boolean_value(false); 693 | BT::ReturnStatus state = root->Tick(); 694 | condition_1->set_boolean_value(true); 695 | 696 | state = root->Tick(); 697 | 698 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 699 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 700 | ASSERT_EQ(BT::RUNNING, state); 701 | 702 | root->Halt(); 703 | } 704 | 705 | TEST_F(ComplexFallbackWithMemoryTest, Conditions2ToTrue) 706 | { 707 | condition_1->set_boolean_value(false); 708 | 709 | condition_2->set_boolean_value(false); 710 | 711 | BT::ReturnStatus state = root->Tick(); 712 | 713 | condition_2->set_boolean_value(true); 714 | 715 | state = root->Tick(); 716 | 717 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 718 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 719 | ASSERT_EQ(BT::RUNNING, state); 720 | 721 | root->Halt(); 722 | } 723 | 724 | TEST_F(ComplexFallbackWithMemoryTest, Action1Failed) 725 | { 726 | action_1->set_boolean_value(false); 727 | condition_1->set_boolean_value(false); 728 | condition_2->set_boolean_value(false); 729 | 730 | BT::ReturnStatus state = root->Tick(); 731 | 732 | state = root->Tick(); 733 | std::this_thread::sleep_for(std::chrono::seconds(5)); 734 | state = root->Tick(); 735 | 736 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 737 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 738 | ASSERT_EQ(BT::RUNNING, state); 739 | 740 | root->Halt(); 741 | } 742 | 743 | 744 | TEST_F(SimpleParallelTest, ConditionsTrue) 745 | { 746 | BT::ReturnStatus state = root->Tick(); 747 | 748 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 749 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 750 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 751 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 752 | ASSERT_EQ(BT::RUNNING, state); 753 | 754 | root->Halt(); 755 | } 756 | 757 | 758 | TEST_F(SimpleParallelTest, Threshold_3) 759 | { 760 | root->set_threshold_M(3); 761 | action_2->set_time(200); 762 | root->Tick(); 763 | std::this_thread::sleep_for(std::chrono::seconds(5)); 764 | BT::ReturnStatus state = root->Tick(); 765 | 766 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 767 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 768 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 769 | ASSERT_EQ(BT::HALTED, action_2->get_status()); 770 | ASSERT_EQ(BT::SUCCESS, state); 771 | 772 | root->Halt(); 773 | } 774 | 775 | 776 | TEST_F(SimpleParallelTest, Threshold_1) 777 | { 778 | root->set_threshold_M(1); 779 | BT::ReturnStatus state = root->Tick(); 780 | 781 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 782 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 783 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 784 | ASSERT_EQ(BT::IDLE, action_2->get_status()); 785 | ASSERT_EQ(BT::SUCCESS, state); 786 | 787 | root->Halt(); 788 | } 789 | TEST_F(ComplexParallelTest, ConditionsTrue) 790 | { 791 | BT::ReturnStatus state = root->Tick(); 792 | 793 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 794 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 795 | ASSERT_EQ(BT::IDLE, condition_3->get_status()); 796 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 797 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 798 | ASSERT_EQ(BT::IDLE, action_3->get_status()); 799 | ASSERT_EQ(BT::RUNNING, parallel_1->get_status()); 800 | ASSERT_EQ(BT::IDLE, parallel_2->get_status()); 801 | ASSERT_EQ(BT::RUNNING, state); 802 | 803 | root->Halt(); 804 | } 805 | 806 | 807 | 808 | TEST_F(ComplexParallelTest, Condition3False) 809 | { 810 | condition_3->set_boolean_value(false); 811 | BT::ReturnStatus state = root->Tick(); 812 | 813 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 814 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 815 | ASSERT_EQ(BT::IDLE, condition_3->get_status()); 816 | ASSERT_EQ(BT::RUNNING, action_1->get_status()); 817 | ASSERT_EQ(BT::RUNNING, action_2->get_status()); 818 | ASSERT_EQ(BT::RUNNING, action_3->get_status()); 819 | ASSERT_EQ(BT::RUNNING, parallel_1->get_status()); 820 | ASSERT_EQ(BT::RUNNING, parallel_2->get_status()); 821 | ASSERT_EQ(BT::RUNNING, state); 822 | 823 | root->Halt(); 824 | } 825 | 826 | 827 | TEST_F(ComplexParallelTest, Condition3FalseAction1Done) 828 | { 829 | action_2->set_time(10); 830 | action_3->set_time(10); 831 | 832 | condition_3->set_boolean_value(false); 833 | BT::ReturnStatus state = root->Tick(); 834 | std::this_thread::sleep_for(std::chrono::seconds(5)); 835 | 836 | 837 | ASSERT_EQ(BT::IDLE, condition_1->get_status()); 838 | ASSERT_EQ(BT::IDLE, condition_2->get_status()); 839 | ASSERT_EQ(BT::IDLE, condition_3->get_status()); 840 | ASSERT_EQ(BT::SUCCESS, action_1->get_status()); // success not read yet by the node parallel_1 841 | ASSERT_EQ(BT::RUNNING, parallel_1->get_status()); // parallel_1 hasn't realize (yet) that action_1 has succeeded 842 | 843 | state = root->Tick(); 844 | 845 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 846 | ASSERT_EQ(BT::IDLE, parallel_1->get_status()); 847 | ASSERT_EQ(BT::HALTED, action_2->get_status()); 848 | ASSERT_EQ(BT::RUNNING, action_3->get_status()); 849 | ASSERT_EQ(BT::RUNNING, parallel_2->get_status()); 850 | ASSERT_EQ(BT::RUNNING, state); 851 | 852 | 853 | state = root->Tick(); 854 | std::this_thread::sleep_for(std::chrono::seconds(15)); 855 | state = root->Tick(); 856 | 857 | ASSERT_EQ(BT::IDLE, parallel_2->get_status()); 858 | ASSERT_EQ(BT::IDLE, action_1->get_status()); 859 | ASSERT_EQ(BT::IDLE, parallel_1->get_status()); 860 | ASSERT_EQ(BT::IDLE, action_3->get_status()); 861 | ASSERT_EQ(BT::IDLE, parallel_2->get_status()); 862 | ASSERT_EQ(BT::SUCCESS, state); 863 | 864 | root->Halt(); 865 | } 866 | 867 | int main(int argc, char **argv) 868 | { 869 | ros::init(argc, argv, "BehaviorTree"); 870 | 871 | // testing::GTEST_FLAG(filter) = "ComplexSequence2ActionsTest.ConditionsTrue"; 872 | testing::InitGoogleTest(&argc, argv); 873 | return RUN_ALL_TESTS(); 874 | } 875 | 876 | 877 | --------------------------------------------------------------------------------