def publish(self, root: py_trees.behaviour.Behaviour, changed: bool,
statistics: py_trees_msgs.Statistics,
visited_behaviour_ids: typing.Set[uuid.UUID],
visited_blackboard_client_ids: typing.Set[uuid.UUID]):
""""
Publish a snapshot, including only what has been parameterised.
Args:
root: the tree
changed: whether the tree status / graph changed or not
statistics: add tree statistics to the snapshot data
visited_behaviour_ids: behaviours on the visited path
visited_blackboard_client_ids: blackboard clients belonging to behaviours on the visited path
"""
if self.last_snapshot_timestamp is None:
changed = True
self.last_snapshot_timestamp = time.monotonic()
current_timestamp = time.monotonic()
elapsed_time = current_timestamp - self.last_snapshot_timestamp
if_its_close_enough = 0.98 # https://github.com/splintered-reality/py_trees_ros/issues/144
if (not changed and elapsed_time <
if_its_close_enough * self.parameters.snapshot_period):
return
tree_message = py_trees_msgs.BehaviourTree()
tree_message.changed = changed
# tree
for behaviour in root.iterate():
msg = conversions.behaviour_to_msg(behaviour)
msg.is_active = True if behaviour.id in visited_behaviour_ids else False
tree_message.behaviours.append(msg)
# blackboard
if self.parameters.blackboard_data:
visited_keys = py_trees.blackboard.Blackboard.keys_filtered_by_clients(
client_ids=visited_blackboard_client_ids)
for key in visited_keys:
try:
value = str(py_trees.blackboard.Blackboard.get(key))
except KeyError:
value = "-"
tree_message.blackboard_on_visited_path.append(
diagnostic_msgs.KeyValue(key=key, value=value))
# activity stream
# TODO: checking the stream is not None is redundant, perhaps use it as an exception check?
if self.parameters.blackboard_activity and py_trees.blackboard.Blackboard.activity_stream is not None:
tree_message.blackboard_activity = conversions.activity_stream_to_msgs(
)
# other
if statistics is not None:
tree_message.statistics = statistics
self.publisher.publish(tree_message)
self.last_snapshot_timestamp = current_timestamp
def _publish_serialised_tree(self):
""""
Args:
tree (:class:`~py_trees.trees_ros.BehaviourTree`): the behaviour tree that has just been ticked
"""
# could just use 'self' here...
tree_message = py_trees_msgs.BehaviourTree()
if self.statistics is not None:
tree_message.statistics = self.statistics
for behaviour in self.root.iterate():
msg = conversions.behaviour_to_msg(behaviour)
msg.is_active = True if behaviour.id in self.snapshot_visitor.visited else False
tree_message.behaviours.append(msg)
self.publishers.snapshots.publish(tree_message)
def initialise(self):
"""
Initialise and stamp a :class:`py_trees_msgs.msg.BehaviourTree`
instance.
"""
self.tree = py_trees_msgs.BehaviourTree()