From d4b53e4ab086aa90868361c1cf41e343114f8592 Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 28 Aug 2025 16:25:54 -0700 Subject: [PATCH 1/3] Refactor blackboard printing to call separate function for ROS messages --- py_trees/display.py | 47 ++++++++++++++++++++++++++++----------------- 1 file changed, 29 insertions(+), 18 deletions(-) diff --git a/py_trees/display.py b/py_trees/display.py index e718b50b..5ab96315 100644 --- a/py_trees/display.py +++ b/py_trees/display.py @@ -20,6 +20,9 @@ # Imports ############################################################################## +# ROS import for super hacky filtering of messages! +import genpy + import os import typing import uuid @@ -815,6 +818,9 @@ def generate_lines( metadata: typing.Optional[typing.Dict[str, blackboard.KeyMetaData]], indent: int, ) -> typing.Iterator[str]: + def format_ros_message(msg: genpy.message.Message): + return "...Got ROS message!" + def assemble_value_line( key: str, value: typing.Any, @@ -822,29 +828,34 @@ def assemble_value_line( indent: str, key_width: int, ) -> str: - s = "" - lines = ("{0}".format(value)).split("\n") + is_ros_message = issubclass(type(value), genpy.message.Message) + if is_ros_message: + value_str = format_ros_message(value) + else: + value_str = "{0}".format(value) + + lines = value_str.split("\n") if len(lines) > 1: - s += ( - console.cyan - + indent - + "{0: <{1}}".format(key, key_width) - + console.white - + ":\n" - ) + lines_str = "\n" for line in lines: - s += console.yellow + indent + " {0}\n".format(line) + lines_str += console.yellow + indent + " {0}\n".format(line) else: - s += ( - console.cyan - + indent - + "{0: <{1}}".format(key, key_width) - + console.white - + ": " - + console.yellow - + "{0}\n".format(value) + lines_str = ( + console.yellow + + value_str + + "\n" + console.reset ) + + s = ( + console.cyan + + indent + + "{0: <{1}}".format(key, key_width) + + console.white + + ":" + + lines_str + ) + return style(s, apply_highlight) + console.reset def assemble_metadata_line( From 11c5c725eb54f5ea84d78ab8bb8065496c0991fb Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 28 Aug 2025 16:25:54 -0700 Subject: [PATCH 2/3] Implement custom printing for ROS messages --- py_trees/display.py | 41 ++++++++++++++++++++++++++++++++++++++--- 1 file changed, 38 insertions(+), 3 deletions(-) diff --git a/py_trees/display.py b/py_trees/display.py index 5ab96315..16876e86 100644 --- a/py_trees/display.py +++ b/py_trees/display.py @@ -818,8 +818,6 @@ def generate_lines( metadata: typing.Optional[typing.Dict[str, blackboard.KeyMetaData]], indent: int, ) -> typing.Iterator[str]: - def format_ros_message(msg: genpy.message.Message): - return "...Got ROS message!" def assemble_value_line( key: str, @@ -852,7 +850,7 @@ def assemble_value_line( + indent + "{0: <{1}}".format(key, key_width) + console.white - + ":" + + ": " + lines_str ) @@ -1160,3 +1158,40 @@ def unicode_blackboard_activity_stream( indent=indent, symbols=unicode_symbols if console.has_unicode() else ascii_symbols, ) + + +def format_ros_message(msg: genpy.message.Message, depth: int = 0): + # return "...Got ROS message!" + msg_str = "" + padding = depth * " " + for slot in msg.__slots__: + value = getattr(msg, slot) + if type(value) == bytes: + msg_str += padding + f"{slot}: bytes \n" + # TODO: handle stamp appropriately! + elif type(value) == genpy.message.Time: + msg_str += ( + f"{padding}{slot}:\n" + + f" {padding}secs: {value.secs}\n" + + f" {padding}nsecs: {value.nsecs}\n" + ) + # TODO: Handle arrays appropriately; will be a bit tricky because + # they can be arrays of primitives OR arrays of messages + elif type(value) == list: + if len(value) == 0: + msg_str += padding + f"{slot}: [] \n" + elif issubclass(type(value[0]), genpy.message.Message): + msg_str += padding + f"{slot}: [ \n" + for item in value: + msg_str += f"{padding} -\n" + msg_str += format_ros_message(item, depth + 2) + msg_str += padding + "] \n" + else: + msg_str += padding + f"{slot}: [{value}] \n" + elif issubclass(type(value), genpy.message.Message): + # TODO: Figure out indentation levels! + msg_str += padding + f"{slot}: \n" + format_ros_message(value, depth + 1) + else: + msg_str += padding + f"{slot}: {value} \n" + return msg_str + From d89723348948567ce1d32d3683a55697484f4eb1 Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 28 Aug 2025 16:25:54 -0700 Subject: [PATCH 3/3] Only print blackboard variables that have *changed* (The ROS blackboard gets way too big) --- py_trees/display.py | 12 ++++++++++++ py_trees/visitors.py | 5 +++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/py_trees/display.py b/py_trees/display.py index 16876e86..7eb5a1c3 100644 --- a/py_trees/display.py +++ b/py_trees/display.py @@ -886,11 +886,23 @@ def assemble_metadata_line( s += console.yellow + "{}\n".format(", ".join(metastrings)) return style(s, apply_highlight) + console.reset + # Filter variables to display based on whether they will have changed since last time + changed_activities = [ + blackboard.ActivityType.WRITE.value, + blackboard.ActivityType.UNSET.value, + blackboard.ActivityType.INITIALISED.value + ] + changed_keys = [ + item.key for item in blackboard.Blackboard.activity_stream.data + if item.activity_type in changed_activities + ] text_indent: str = _symbols["space"] * (4 + indent) key_width: int = 0 for key in storage.keys(): key_width = len(key) if len(key) > key_width else key_width for key in sorted(storage.keys()): + if key not in changed_keys: + continue if metadata is not None: yield assemble_metadata_line( key=key, diff --git a/py_trees/visitors.py b/py_trees/visitors.py index 8e3d44d7..19815b69 100644 --- a/py_trees/visitors.py +++ b/py_trees/visitors.py @@ -219,6 +219,9 @@ def run(self, behaviour: behaviour.Behaviour) -> None: def finalise(self) -> None: """Print a summary on stdout after all behaviours have been visited.""" + if self.display_blackboard: + print(display.unicode_blackboard(key_filter=self.visited_blackboard_keys)) + if self.root is not None: print( "\n" @@ -230,7 +233,5 @@ def finalise(self) -> None: previously_visited=self.previously_visited, ) ) - if self.display_blackboard: - print(display.unicode_blackboard(key_filter=self.visited_blackboard_keys)) if self.display_activity_stream: print(display.unicode_blackboard_activity_stream())