def test_replace_behaviour_tree(): console.banner("Replace Behaviour Subtree") a = py_trees.behaviours.Count(name="A") sequence1 = py_trees.Sequence(name="Sequence1") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence1.add_child(b) sequence1.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence1) root.add_child(d) tree = py_trees.BehaviourTree(root) py_trees.display.print_ascii_tree(tree.root) assert (len(sequence1.children) == 2) sequence2 = py_trees.Sequence(name="Sequence2") e = py_trees.behaviours.Count(name="E") f = py_trees.behaviours.Count(name="F") g = py_trees.behaviours.Count(name="G") sequence2.add_child(e) sequence2.add_child(f) sequence2.add_child(g) tree.replace_subtree(sequence1.id, sequence2) py_trees.display.print_ascii_tree(tree.root) assert (len(sequence2.children) == 3)
def test_tick_tock_behaviour_tree(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Tick Tock Behaviour Tree" + console.reset) print( console.bold + "****************************************************************************************\n" + console.reset) a = py_trees.behaviours.Count(name="A") sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence) root.add_child(d) tree = py_trees.BehaviourTree(root) py_trees.display.print_ascii_tree(tree.root) visitor = py_trees.trees.DebugVisitor() tree.visitors.append(visitor) tree.tick_tock(100, 5, pre_tick_handler=pre_tick_visitor) print("\n--------- Assertions ---------\n") print("a.status == py_trees.Status.RUNNING") assert (a.status == py_trees.Status.RUNNING)
def test_prune_behaviour_tree(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Prune Behaviour Tree" + console.reset) print( console.bold + "****************************************************************************************\n" + console.reset) a = py_trees.behaviours.Count(name="A") sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence) root.add_child(d) tree = py_trees.BehaviourTree(root) py_trees.display.print_ascii_tree(tree.root) assert (len(sequence.children) == 2) tree.prune_subtree(c.id) py_trees.display.print_ascii_tree(tree.root) assert (len(sequence.children) == 1) tree.prune_subtree(sequence.id) py_trees.display.print_ascii_tree(tree.root) assert (len(root.children) == 2)
def test_full_iteration(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Visit Whole Tree" + console.reset) print( console.bold + "****************************************************************************************\n" + console.reset) visitor = py_trees.trees.DebugVisitor() a = py_trees.behaviours.Count(name="A") sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence) root.add_child(d) visitations = 0 for child in root.iterate(): visitations += 1 child.visit(visitor) assert (visitations == 6)
def test_display(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Display Tree" + console.reset) print( console.bold + "****************************************************************************************\n" + console.reset) a = py_trees.behaviours.Count(name="A") sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence) root.add_child(d) py_trees.display.print_ascii_tree(root) assert (True)
def __init__(self, *args): super(TestSubscriberNestedCheck, self).__init__(*args) generate_failure = rospy.get_param("~failure", False) bad_topic = rospy.get_param("~bad_topic", False) topic_name = rospy.get_param( "~topic_name", "pose_chatter") if not bad_topic else "wrong_topic" expected_value = 1.0 if not generate_failure else -1.0 self.logger = py_trees.logging.get_logger("Subscriber Check") self.root = py_trees.Sequence(name="Demo Subscribers") check_subscriber_variable = py_trees.subscribers.CheckSubscriberVariable( "Check", topic_name=topic_name, topic_type=Pose, variable_name="position.x", expected_value=expected_value, comparison_operator=operator.eq) wait_for_subscriber = py_trees.subscribers.WaitForSubscriberData( "Wait", topic_name=topic_name, topic_type=Pose, ) self.root.add_child(check_subscriber_variable) self.root.add_child(wait_for_subscriber) self.tree = py_trees.ros.BehaviourTree(self.root) rospy.on_shutdown(self.shutdown) # py_trees.display.render_dot_tree(self.root) py_trees.display.print_ascii_tree(self.root, 0)
def test_inverter_tree(): console.banner("Inverter Tree") root = py_trees.Sequence(name="Root") selector = py_trees.Selector(name="Selector") failure = py_trees.behaviours.Failure(name="Failure") failure2 = py_trees.meta.inverter( py_trees.behaviours.Success)(name="Failure2") success = py_trees.behaviours.Success(name="Success") success2 = py_trees.meta.inverter( py_trees.behaviours.Failure)(name="Success2") selector.add_child(failure) selector.add_child(failure2) selector.add_child(success) root.add_child(selector) root.add_child(success2) py_trees.display.print_ascii_tree(root) visitor = py_trees.visitors.DebugVisitor() py_trees.tests.tick_tree(root, visitor, 1, 1) print("\n--------- Assertions ---------\n") print("success.status == py_trees.Status.SUCCESS") assert (success.status == py_trees.Status.SUCCESS) print("success2.status == py_trees.Status.SUCCESS") assert (success2.status == py_trees.Status.SUCCESS) print("root.status == py_trees.Status.SUCCESS") assert (root.status == py_trees.Status.SUCCESS) print("failure.status == py_trees.Status.FAILURE") assert (failure.status == py_trees.Status.FAILURE) print("failure2.status == py_trees.Status.FAILURE") assert (failure2.status == py_trees.Status.FAILURE)
def test_sequence_composite(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Sequence" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) visitor = py_trees.visitors.DebugVisitor() tree = py_trees.Sequence(name='Sequence') a = py_trees.behaviours.Count(name="A", fail_until=0, running_until=3, success_until=6) b = py_trees.behaviours.Count(name="B", fail_until=0, running_until=3, success_until=6) c = py_trees.behaviours.Count(name="C", fail_until=0, running_until=3, success_until=6) tree.add_child(a) tree.add_child(b) tree.add_child(c) py_trees.tests.tick_tree(tree, visitor, 1, 5) py_trees.tests.print_summary(nodes=[a, b, c]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.SUCCESS") assert (a.status == py_trees.Status.SUCCESS) print("b.status == py_trees.Status.RUNNING") assert (b.status == py_trees.Status.RUNNING) print("tree.status == py_trees.Status.RUNNING") assert (tree.status == py_trees.Status.RUNNING) py_trees.tests.tick_tree(tree, visitor, 6, 10) py_trees.tests.print_summary(nodes=[a, b, c]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.SUCCESS") assert (a.status == py_trees.Status.SUCCESS) print("b.status == py_trees.Status.SUCCESS") assert (b.status == py_trees.Status.SUCCESS) print("b.status == py_trees.Status.SUCCESS") assert (c.status == py_trees.Status.SUCCESS) print("tree.status == py_trees.Status.SUCCESS") assert (tree.status == py_trees.Status.SUCCESS) py_trees.tests.tick_tree(tree, visitor, 11, 13) py_trees.tests.print_summary(nodes=[a, b, c]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.RUNNING") assert (a.status == py_trees.Status.RUNNING) print("b.status == py_trees.Status.INVALID") assert (b.status == py_trees.Status.INVALID) print("c.status == py_trees.Status.INVALID") assert (c.status == py_trees.Status.INVALID) print("tree.status == py_trees.Status.RUNNING") assert (tree.status == py_trees.Status.RUNNING)
def create_tree(): every_n_success = py_trees.behaviours.SuccessEveryN("EveryN", 5) sequence = py_trees.Sequence(name="Sequence") guard = py_trees.behaviours.Success("Guard") periodic_success = py_trees.behaviours.Periodic("Periodic", 3) finisher = py_trees.behaviours.Success("Finisher") sequence.add_child(guard) sequence.add_child(periodic_success) sequence.add_child(finisher) sequence.blackbox_level = py_trees.common.BlackBoxLevel.COMPONENT idle = py_trees.behaviours.Success("Idle") root = py_trees.Selector(name="Demo Tree") root.add_child(every_n_success) root.add_child(sequence) root.add_child(idle) return root
def __init__(self): self.logger = py_trees.logging.Logger("Testies") self.root = py_trees.Sequence(name="Root") self.action_client = py_trees_ros.actions.ActionClient( "Action Client", action_spec=move_base_msgs.msg.MoveBaseAction, action_goal=move_base_msgs.msg.MoveBaseGoal(), action_namespace="move_base", ) self.root.add_child(self.action_client) self.tree = py_trees_ros.trees.BehaviourTree(self.root) self.tree.visitors.append(py_trees.visitors.DebugVisitor()) self.tree.add_pre_tick_handler(self.pre_tick_handler) print(console.bold + "\nTree\n" + console.reset) py_trees.display.print_ascii_tree(self.root, 0) self.tree.setup(15) rospy.on_shutdown(self.shutdown)
def test_display(): console.banner("Display Tree") a = py_trees.behaviours.Count(name="A") sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence) root.add_child(d) py_trees.display.print_ascii_tree(root) assert (True)
def __init__(self): generate_failure = rospy.get_param("~failure", False) bad_topic = rospy.get_param("~bad_topic", False) topic_name = rospy.get_param( "~topic_name", "chatter") if not bad_topic else "wrong_topic" expected_string = "Hello Dude" if not generate_failure else "Wrong String" print("") print(console.bold + "Parameters" + console.reset) print(console.cyan + " generate_failure: " + console.yellow + str(generate_failure) + console.reset) print(console.cyan + " bad_topic : " + console.yellow + str(bad_topic) + console.reset) print(console.cyan + " topic_name : " + console.yellow + topic_name + console.reset) print(console.cyan + " expected_string : " + console.yellow + expected_string + console.reset) print("") self.logger = py_trees.logging.Logger("Testies") self.root = py_trees.Sequence(name="Root") check_subscriber_variable = py_trees_ros.subscribers.CheckData( "Check", topic_name=topic_name, topic_type=std_msgs.String, variable_name="data", expected_value=expected_string, comparison_operator=operator.eq) wait_for_subscriber = py_trees_ros.subscribers.WaitForData( "Wait", topic_name=topic_name, topic_type=std_msgs.String, ) self.root.add_child(check_subscriber_variable) self.root.add_child(wait_for_subscriber) self.tree = py_trees_ros.trees.BehaviourTree(self.root) self.tree.visitors.append(py_trees.visitors.DebugVisitor()) self.tree.add_pre_tick_handler(self.pre_tick_handler) print(console.bold + "\nTree\n" + console.reset) py_trees.display.print_ascii_tree(self.root, 0) self.tree.setup(15) rospy.on_shutdown(self.shutdown)
def test_full_iteration(): console.banner("Visit Whole Tree") visitor = py_trees.visitors.DebugVisitor() a = py_trees.behaviours.Count(name="A") sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B") c = py_trees.behaviours.Count(name="C") sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D") root = py_trees.Selector(name="Root") root.add_child(a) root.add_child(sequence) root.add_child(d) visitations = 0 for child in root.iterate(): visitations += 1 child.visit(visitor) assert (visitations == 6)
def test_mixed_tree(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Mixed Tree" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) visitor = py_trees.trees.DebugVisitor() a = py_trees.behaviours.Count(name="A", fail_until=3, running_until=5, success_until=7) sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B", fail_until=0, running_until=3, success_until=5) c = py_trees.behaviours.Count(name="C", fail_until=0, running_until=3, success_until=5) sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D", fail_until=0, running_until=3, success_until=15) root = py_trees.Selector(name="Root") print("Root.children: %s " % [child.name for child in root.children]) root.add_child(a) root.add_child(sequence) root.add_child(d) tick_tree(root, visitor, 1, 2) print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.RUNNING") assert (sequence.status == py_trees.Status.RUNNING) print("b.status == py_trees.Status.RUNNING") assert (b.status == py_trees.Status.RUNNING) print("root.status == py_trees.Status.RUNNING") assert (root.status == py_trees.Status.RUNNING) tick_tree(root, visitor, 3, 11) print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.RUNNING") assert (sequence.status == py_trees.Status.RUNNING) print("c.status == py_trees.Status.RUNNING") assert (c.status == py_trees.Status.RUNNING) print("root.status == py_trees.Status.RUNNING") assert (root.status == py_trees.Status.RUNNING) tick_tree(root, visitor, 12, 14) print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.SUCCESS") assert (sequence.status == py_trees.Status.SUCCESS) print("c.status == py_trees.Status.SUCCESS") assert (c.status == py_trees.Status.SUCCESS) print("root.status == py_trees.Status.SUCCESS") assert (root.status == py_trees.Status.SUCCESS) tick_tree(root, visitor, 15, 16) print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.FAILURE") assert (sequence.status == py_trees.Status.FAILURE) print("b.status == py_trees.Status.FAILURE") assert (b.status == py_trees.Status.FAILURE) print("d.status == py_trees.Status.RUNNING") assert (d.status == py_trees.Status.RUNNING) print("root.status == py_trees.Status.RUNNING") assert (root.status == py_trees.Status.RUNNING)
def test_mixed_tree(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Mixed Tree" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) visitor = py_trees.visitors.DebugVisitor() a = py_trees.behaviours.Count(name="A", fail_until=3, running_until=5, success_until=7, reset=False) sequence = py_trees.Sequence(name="Sequence") b = py_trees.behaviours.Count(name="B", fail_until=0, running_until=3, success_until=5, reset=False) c = py_trees.behaviours.Count(name="C", fail_until=0, running_until=3, success_until=5, reset=False) sequence.add_child(b) sequence.add_child(c) d = py_trees.behaviours.Count(name="D", fail_until=0, running_until=3, success_until=15) root = py_trees.Selector(name="Selector") root.add_child(a) root.add_child(sequence) root.add_child(d) py_trees.display.print_ascii_tree(root) py_trees.tests.tick_tree(root, visitor, 1, 2) py_trees.tests.print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.RUNNING") assert (sequence.status == py_trees.Status.RUNNING) print("b.status == py_trees.Status.RUNNING") assert (b.status == py_trees.Status.RUNNING) print("root.status == py_trees.Status.RUNNING") assert (root.status == py_trees.Status.RUNNING) py_trees.tests.tick_tree(root, visitor, 3, 9) py_trees.tests.print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.RUNNING") assert (sequence.status == py_trees.Status.RUNNING) print("c.status == py_trees.Status.RUNNING") assert (c.status == py_trees.Status.RUNNING) print("root.status == py_trees.Status.RUNNING") assert (root.status == py_trees.Status.RUNNING) py_trees.tests.tick_tree(root, visitor, 10, 11) py_trees.tests.print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.SUCCESS") assert (sequence.status == py_trees.Status.SUCCESS) print("c.status == py_trees.Status.SUCCESS") assert (c.status == py_trees.Status.SUCCESS) print("root.status == py_trees.Status.SUCCESS") assert (root.status == py_trees.Status.SUCCESS) py_trees.tests.tick_tree(root, visitor, 12, 13) py_trees.tests.print_summary(nodes=[a, b, c, d]) print("--------- Assertions ---------\n") print("a.status == py_trees.Status.FAILURE") assert (a.status == py_trees.Status.FAILURE) print("sequence.status == py_trees.Status.FAILURE") assert (sequence.status == py_trees.Status.FAILURE) print("b.status == py_trees.Status.FAILURE") assert (b.status == py_trees.Status.FAILURE) print("d.status == py_trees.Status.RUNNING") assert (d.status == py_trees.Status.RUNNING) print("root.status == py_trees.Status.RUNNING") assert (root.status == py_trees.Status.RUNNING)