def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
# invasive hack to revert the dummy child we added in msg_to_behaviour
if isinstance(behaviour, py_trees.decorators.Decorator):
behaviour.children = [child]
behaviour.decorated = behaviour.children[0]
else:
behaviour.children.append(child)
child.parent = behaviour
# set the current child so tip() works properly everywhere
if behaviour.children:
if msg.current_child_id != unique_identifier_msgs.UUID():
current_child_id = conversions.msg_to_uuid4(
msg.current_child_id)
for index, child in enumerate(behaviour.children):
if child.id == current_child_id:
# somewhat ugly not having a consistent api here
if isinstance(behaviour,
py_trees.composites.Selector):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Sequence):
behaviour.current_index = index
# else Parallel, nothing to do since it infers
# the current child from children's status on the fly
break
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
def uuid4_to_msg(uuid4: uuid.UUID = uuid.uuid4()):
"""
Convert a uuid4 python object to a ros unique identifier, UUID type.
Args:
uuid4 (:class:`uuid.UUID`), optional: unique identifier to convert, defaults to auto-generated uuid4
Returns:
:class:`unique_identifier_msgs.msg.UUID`: the ros message type
"""
return unique_identifier_msgs.UUID(uuid=list(uuid4.bytes))
def behaviour_to_msg(behaviour):
"""
Convert a behaviour to a message.
Args:
behaviour (:class:`~py_trees.behaviour.Behaviour`): behaviour to convert
Returns:
:class:`~py_trees_msgs.msg.Behaviour`: a ros message representation of a behaviour
"""
msg = py_trees_msgs.Behaviour()
msg.name = behaviour.name
msg.class_name = str(behaviour.__module__) + '.' + str(
type(behaviour).__name__)
msg.own_id = uuid4_to_msg(behaviour.id)
msg.parent_id = uuid4_to_msg(
behaviour.parent.id
) if behaviour.parent else unique_identifier_msgs.UUID()
msg.child_ids = [
uuid4_to_msg(child.id)
for child in behaviour.iterate(direct_descendants=True)
if not child.id == behaviour.id
]
tip = behaviour.tip()
# tip_id is empty if the behaviour is invalid or if it is a valid
# leaf
if tip is not None and tip != behaviour:
msg.tip_id = uuid4_to_msg(tip.id)
msg.type = behaviour_type_to_msg_constant(behaviour)
msg.blackbox_level = blackbox_enum_to_msg_constant(
behaviour.blackbox_level)
msg.status = status_enum_to_msg_constant(behaviour.status)
msg.message = behaviour.feedback_message
return msg
def callback_snapshot(self, msg):
"""
Formats the string message coming in.
Args
msg (:class:`py_trees_ros_interfaces.msg.BehaviourTree`):serialised snapshot
"""
####################
# Processing
####################
self.snapshot_visitor.previously_visited = self.snapshot_visitor.visited
self.snapshot_visitor.visited = {}
serialised_behaviours = {}
root_id = None
for serialised_behaviour in msg.behaviours:
if serialised_behaviour.parent_id == unique_identifier_msgs.UUID():
root_id = conversions.msg_to_uuid4(serialised_behaviour.own_id)
serialised_behaviours[conversions.msg_to_uuid4(
serialised_behaviour.own_id)] = serialised_behaviour
def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
# invasive hack to revert the dummy child we added in msg_to_behaviour
if isinstance(behaviour, py_trees.decorators.Decorator):
behaviour.children = [child]
behaviour.decorated = behaviour.children[0]
else:
behaviour.children.append(child)
child.parent = behaviour
# set the current child so tip() works properly everywhere
if behaviour.children:
if msg.current_child_id != unique_identifier_msgs.UUID():
current_child_id = conversions.msg_to_uuid4(
msg.current_child_id)
for index, child in enumerate(behaviour.children):
if child.id == current_child_id:
# somewhat ugly not having a consistent api here
if isinstance(behaviour,
py_trees.composites.Selector):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Sequence):
behaviour.current_index = index
# else Parallel, nothing to do since it infers
# the current child from children's status on the fly
break
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
# we didn't set the tip in any behaviour, but nothing depends
# on that right now
root = deserialise_tree_recursively(serialised_behaviours[root_id])
####################
# Streaming
####################
if self.mode == WatcherMode.SNAPSHOTS:
if msg.changed:
colour = console.green
else:
colour = console.bold
####################
# Banner
####################
title = "Tick {}".format(msg.statistics.count)
print(colour + "\n" + 80 * "*" + console.reset)
print(colour + "* " + console.bold + title.center(80) +
console.reset)
print(colour + 80 * "*" + "\n" + console.reset)
####################
# Tree Snapshot
####################
print(
py_trees.display.unicode_tree(
root=root,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
print(colour + "-" * 80 + console.reset)
####################
# Stream Variables
####################
if self.parameters.blackboard_data:
print("")
print(colour + "Blackboard Data" + console.reset)
indent = " " * 4
if not msg.blackboard_on_visited_path:
print(console.cyan + indent + "-" + console.reset + " : " +
console.yellow + "-" + console.reset)
else:
# could probably re-use the unicode_blackboard by passing a dict to it
# like we've done for the activity stream
max_length = 0
for variable in msg.blackboard_on_visited_path:
max_length = len(variable.key) if len(
variable.key) > max_length else max_length
for variable in msg.blackboard_on_visited_path:
print(console.cyan + indent +
'{0: <{1}}'.format(variable.key, max_length +
1) + console.reset + ": " +
console.yellow + '{0}'.format(variable.value) +
console.reset)
####################
# Stream Activity
####################
if self.parameters.blackboard_activity:
print("")
print(colour + "Blackboard Activity Stream" + console.reset)
if msg.blackboard_activity:
print(
py_trees.display.unicode_blackboard_activity_stream(
msg.blackboard_activity,
indent=0,
show_title=False))
else:
indent = " " * 4
print(console.cyan + indent + "-" + console.reset + " : " +
console.yellow + "-" + console.reset)
####################
# Stream Statistics
####################
if self.statistics:
print("")
print(colour + "Statistics" + console.reset)
print(console.cyan + " Timestamp: " + console.yellow +
"{}".format(
conversions.rclpy_time_to_float(
rclpy.time.Time.from_msg(msg.statistics.stamp))))
print(console.cyan + " Duration : " + console.yellow +
"{:.3f}/{:.3f}/{:.3f} (ms) [time/avg/stddev]".format(
msg.statistics.tick_duration *
1000, msg.statistics.tick_duration_average * 1000,
math.sqrt(msg.statistics.tick_duration_variance) *
1000))
print(console.cyan + " Interval : " + console.yellow +
"{:.3f}/{:.3f}/{:.3f} (s) [time/avg/stddev]".format(
msg.statistics.tick_interval,
msg.statistics.tick_interval_average,
math.sqrt(msg.statistics.tick_interval_variance)))
print(console.reset)
####################
# Dot Graph
####################
elif self.mode == WatcherMode.DOT_GRAPH and not self.rendered:
self.rendered = True
directory_name = tempfile.mkdtemp()
py_trees.display.render_dot_tree(
root=root,
target_directory=directory_name,
with_blackboard_variables=self.parameters.blackboard_data)
xdot_program = py_trees.utilities.which('xdot')
if not xdot_program:
print("")
console.logerror(
"No xdot viewer found [hint: sudo apt install xdot]")
print("")
print(py_trees.display.dot_tree(root=root).to_string())
self.done = True
self.xdot_process = None
return
filename = py_trees.utilities.get_valid_filename(
root.name) + '.dot'
if xdot_program:
try:
self.xdot_process = subprocess.Popen(
[xdot_program,
os.path.join(directory_name, filename)])
except KeyboardInterrupt:
pass
self.done = True
def callback_snapshot(self, msg):
"""
Formats the string message coming in.
Args
msg (:class:`py_trees_ros_interfaces.msg.BehaviourTree`):serialised snapshot
"""
####################
# Processing
####################
self.snapshot_visitor.previously_visited = self.snapshot_visitor.visited
self.snapshot_visitor.visited = {}
serialised_behaviours = {}
root_id = None
for serialised_behaviour in msg.behaviours:
if serialised_behaviour.parent_id == unique_identifier_msgs.UUID():
root_id = conversions.msg_to_uuid4(serialised_behaviour.own_id)
serialised_behaviours[conversions.msg_to_uuid4(
serialised_behaviour.own_id)] = serialised_behaviour
def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
child.parent = behaviour
behaviour.children.append(child)
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
# we didn't set the tip in any behaviour, but nothing depends
# on that right now
root = deserialise_tree_recursively(serialised_behaviours[root_id])
####################
# Streaming
####################
if self.viewing_mode == WatcherMode.ASCII_SNAPSHOT:
console.banner("Tick {}".format(msg.statistics.count))
print(
py_trees.display.ascii_tree(
root=root,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
print(console.green + "-" * 80 + "\n" + console.reset)
print("Timestamp: {}".format(
conversions.rclpy_time_to_float(
rclpy.time.Time.from_msg(msg.statistics.stamp))))
print("Duration : {:.3f}/{:.3f}/{:.3f} (ms) [time/avg/stddev]".
format(
msg.statistics.tick_duration * 1000,
msg.statistics.tick_duration_average * 1000,
math.sqrt(msg.statistics.tick_duration_variance) * 1000))
print(
"Interval : {:.3f}/{:.3f}/{:.3f} (s) [time/avg/stddev]".format(
msg.statistics.tick_interval,
msg.statistics.tick_interval_average,
math.sqrt(msg.statistics.tick_interval_variance)))
####################
# Printing
####################
elif self.viewing_mode == WatcherMode.ASCII_TREE:
print("")
print(
py_trees.display.ascii_tree(
root=root,
show_status=True,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
self.done = True
####################
# Dot Graph
####################
elif self.viewing_mode == WatcherMode.DOT_TREE and not self.rendered:
self.rendered = True
directory_name = tempfile.mkdtemp()
py_trees.display.render_dot_tree(root=root,
target_directory=directory_name)
xdot_program = py_trees.utilities.which('xdot')
if not xdot_program:
console.logerror(
"No xdot viewer found, skipping display [hint: sudo apt install xdot]"
)
print(py_trees.display.dot_graph(root=root).to_string())
self.done = True
self.xdot_process = 1
return
filename = py_trees.utilities.get_valid_filename(
root.name) + '.dot'
if xdot_program:
try:
self.xdot_process = subprocess.Popen(
[xdot_program,
os.path.join(directory_name, filename)])
except KeyboardInterrupt:
pass
self.done = True
def callback_snapshot(self, msg):
"""
Formats the string message coming in.
Args
msg (:class:`py_trees_ros_interfaces.msg.BehaviourTree`):serialised snapshot
"""
####################
# Processing
####################
self.snapshot_visitor.previously_visited = self.snapshot_visitor.visited
self.snapshot_visitor.visited = {}
serialised_behaviours = {}
root_id = None
for serialised_behaviour in msg.behaviours:
if serialised_behaviour.parent_id == unique_identifier_msgs.UUID():
root_id = conversions.msg_to_uuid4(serialised_behaviour.own_id)
serialised_behaviours[conversions.msg_to_uuid4(
serialised_behaviour.own_id)] = serialised_behaviour
def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
# invasive hack to revert the dummy child we added in msg_to_behaviour
if isinstance(behaviour, py_trees.decorators.Decorator):
behaviour.children = [child]
behaviour.decorated = behaviour.children[0]
else:
behaviour.children.append(child)
child.parent = behaviour
# set the current child so tip() works properly everywhere
if behaviour.children:
if msg.current_child_id != unique_identifier_msgs.UUID():
current_child_id = conversions.msg_to_uuid4(
msg.current_child_id)
for index, child in enumerate(behaviour.children):
if child.id == current_child_id:
# somewhat ugly not having a consistent api here
if isinstance(behaviour,
py_trees.composites.Selector):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Chooser):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Sequence):
behaviour.current_index = index
# else Parallel, nothing to do since it infers
# the current child from children's status on the fly
break
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
# we didn't set the tip in any behaviour, but nothing depends
# on that right now
root = deserialise_tree_recursively(serialised_behaviours[root_id])
####################
# Streaming
####################
if self.viewing_mode == WatcherMode.STREAM:
console.banner("Tick {}".format(msg.statistics.count))
print(
py_trees.display.unicode_tree(
root=root,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
print(console.green + "-" * 80 + "\n" + console.reset)
print("Timestamp: {}".format(
conversions.rclpy_time_to_float(
rclpy.time.Time.from_msg(msg.statistics.stamp))))
print("Duration : {:.3f}/{:.3f}/{:.3f} (ms) [time/avg/stddev]".
format(
msg.statistics.tick_duration * 1000,
msg.statistics.tick_duration_average * 1000,
math.sqrt(msg.statistics.tick_duration_variance) * 1000))
print(
"Interval : {:.3f}/{:.3f}/{:.3f} (s) [time/avg/stddev]".format(
msg.statistics.tick_interval,
msg.statistics.tick_interval_average,
math.sqrt(msg.statistics.tick_interval_variance)))
####################
# Printing
####################
elif self.viewing_mode == WatcherMode.SNAPSHOT:
print("")
print(
py_trees.display.unicode_tree(
root=root,
show_status=True,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
self.done = True
####################
# Dot Graph
####################
elif self.viewing_mode == WatcherMode.DOT_GRAPH and not self.rendered:
self.rendered = True
directory_name = tempfile.mkdtemp()
py_trees.display.render_dot_tree(root=root,
target_directory=directory_name)
xdot_program = py_trees.utilities.which('xdot')
if not xdot_program:
print("")
console.logerror(
"No xdot viewer found [hint: sudo apt install xdot]")
print("")
print(py_trees.display.dot_tree(root=root).to_string())
self.done = True
self.xdot_process = None
return
filename = py_trees.utilities.get_valid_filename(
root.name) + '.dot'
if xdot_program:
try:
self.xdot_process = subprocess.Popen(
[xdot_program,
os.path.join(directory_name, filename)])
except KeyboardInterrupt:
pass
self.done = True