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)
