Add --content-filter support to ros2 topic echo|hz|bw

Add DDS content filter expression support to `ros2 topic echo`,
`ros2 topic hz`, and `ros2 topic bw` commands via new
`--content-filter` and `--content-filter-params` CLI arguments.

Content filters are applied at the middleware level, so only matching
messages are delivered to the subscriber — reducing bandwidth and CPU
usage compared to client-side Python filtering.

Closes #1126

Signed-off-by: Pavel Guzenfeld <pavelguzenfeld@gmail.com>

Author: PavelGuzenfeld

ros2topic/ros2topic/verb/bw.py

@@ -35,6 +35,7 @@
 
 import rclpy
 from rclpy.qos import qos_profile_sensor_data
+from rclpy.subscription_content_filter_options import ContentFilterOptions
 from ros2cli.node.direct import add_arguments as add_direct_node_arguments
 from ros2cli.node.direct import DirectNode
 from ros2topic.api import get_msg_class
@@ -77,11 +78,24 @@ def add_arguments(self, parser, cli_name):
             '--window', '-w', type=positive_int, default=DEFAULT_WINDOW_SIZE,
             help='maximum window size, in # of messages, for calculating rate '
                  f'(default: {DEFAULT_WINDOW_SIZE})', metavar='WINDOW')
+        parser.add_argument(
+            '--content-filter', dest='content_filter_expr', default=None,
+            help='DDS content filter expression applied at the middleware level')
+        parser.add_argument(
+            '--content-filter-params', dest='content_filter_params', nargs='*', default=[],
+            help='Parameters for the content filter expression')
         add_direct_node_arguments(parser)
 
     def main(self, *, args):
+        content_filter_options = None
+        if args.content_filter_expr:
+            content_filter_options = ContentFilterOptions(
+                filter_expression=args.content_filter_expr,
+                expression_parameters=args.content_filter_params)
         with DirectNode(args) as node:
-            return _rostopic_bw(node.node, args.topic, window_size=args.window)
+            return _rostopic_bw(
+                node.node, args.topic, window_size=args.window,
+                content_filter_options=content_filter_options)
 
 
 class ROSTopicBandwidth(object):
@@ -157,7 +171,7 @@ def print_bw(self):
         print(f'{bw} from {n} messages\n\tMessage size mean: {mean} min: {min_s} max: {max_s}')
 
 
-def _rostopic_bw(node, topic, window_size=DEFAULT_WINDOW_SIZE):
+def _rostopic_bw(node, topic, window_size=DEFAULT_WINDOW_SIZE, content_filter_options=None):
     """Periodically print the received bandwidth of a topic to console until shutdown."""
     # pause bw until topic is published
     msg_class = get_msg_class(node, topic, blocking=True, include_hidden_topics=True)
@@ -171,7 +185,8 @@ def _rostopic_bw(node, topic, window_size=DEFAULT_WINDOW_SIZE):
         topic,
         rt.callback,
         qos_profile_sensor_data,
-        raw=True
+        raw=True,
+        content_filter_options=content_filter_options
     )
 
     print(f'Subscribed to [{topic}]')

ros2topic/ros2topic/verb/echo.py

@@ -23,6 +23,7 @@
 from rclpy.qos import QoSPresetProfiles
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy
+from rclpy.subscription_content_filter_options import ContentFilterOptions
 from rclpy.task import Future
 from ros2cli.helpers import unsigned_int
 from ros2cli.node.strategy import add_arguments as add_strategy_node_arguments
@@ -99,6 +100,15 @@ def add_arguments(self, parser, cli_name):
             '--filter', dest='filter_expr', help='Python expression to filter messages that '
                                                  'are printed. Expression can use Python builtins '
                                                  'as well as m (the message).')
+        parser.add_argument(
+            '--content-filter', dest='content_filter_expr', default=None,
+            help='DDS content filter expression applied at the middleware level '
+                 '(e.g. "data > 100"). Only messages matching the filter are '
+                 'delivered to the subscriber. Use %%0, %%1, ... as parameter '
+                 'placeholders with --content-filter-params.')
+        parser.add_argument(
+            '--content-filter-params', dest='content_filter_params', nargs='*', default=[],
+            help='Parameters for the content filter expression')
         parser.add_argument(
             '--once', action='store_true', help='Print the first message received and then exit.')
         parser.add_argument(
@@ -216,13 +226,20 @@ def main(self, *, args):
             if args.timeout is not None:
                 self.timer = node.create_timer(args.timeout, self._timed_out)
 
+            content_filter_options = None
+            if args.content_filter_expr:
+                content_filter_options = ContentFilterOptions(
+                    filter_expression=args.content_filter_expr,
+                    expression_parameters=args.content_filter_params)
+
             self.subscribe_and_spin(
                 node,
                 args.topic_name,
                 message_type,
                 qos_profile,
                 args.no_lost_messages,
-                args.raw)
+                args.raw,
+                content_filter_options=content_filter_options)
 
     def subscribe_and_spin(
         self,
@@ -232,6 +249,7 @@ def subscribe_and_spin(
         qos_profile: QoSProfile,
         no_report_lost_messages: bool,
         raw: bool,
+        content_filter_options: Optional[ContentFilterOptions] = None,
     ) -> Optional[str]:
         """Initialize a node with a single subscription and spin."""
         event_callbacks = None
@@ -245,7 +263,8 @@ def subscribe_and_spin(
                 self._subscriber_callback,
                 qos_profile,
                 event_callbacks=event_callbacks,
-                raw=raw)
+                raw=raw,
+                content_filter_options=content_filter_options)
         except UnsupportedEventTypeError:
             assert not no_report_lost_messages
             node.create_subscription(
@@ -254,7 +273,8 @@ def subscribe_and_spin(
                 self._subscriber_callback,
                 qos_profile,
                 event_callbacks=None,
-                raw=raw)
+                raw=raw,
+                content_filter_options=content_filter_options)
 
         if self.future is not None:
             rclpy.spin_until_future_complete(node, self.future)

ros2topic/ros2topic/verb/hz.py

@@ -40,6 +40,7 @@
 from rclpy.clock import Clock
 from rclpy.clock import ClockType
 from rclpy.qos import qos_profile_sensor_data
+from rclpy.subscription_content_filter_options import ContentFilterOptions
 from ros2cli.node.direct import add_arguments as add_direct_node_arguments
 from ros2cli.node.direct import DirectNode
 from ros2topic.api import get_msg_class
@@ -80,6 +81,12 @@ def add_arguments(self, parser, cli_name):
             dest='use_wtime', default=False, action='store_true',
             help='calculates rate using wall time which can be helpful'
                  ' when clock is not published during simulation')
+        parser.add_argument(
+            '--content-filter', dest='content_filter_expr', default=None,
+            help='DDS content filter expression applied at the middleware level')
+        parser.add_argument(
+            '--content-filter-params', dest='content_filter_params', nargs='*', default=[],
+            help='Parameters for the content filter expression')
         add_direct_node_arguments(parser)
 
     def main(self, *, args):
@@ -97,10 +104,16 @@ def eval_fn(m):
     else:
         filter_expr = None
 
+    content_filter_options = None
+    if args.content_filter_expr:
+        content_filter_options = ContentFilterOptions(
+            filter_expression=args.content_filter_expr,
+            expression_parameters=args.content_filter_params)
+
     with DirectNode(args) as node:
         return _rostopic_hz(
             node.node, topic, window_size=args.window_size, filter_expr=filter_expr,
-            use_wtime=args.use_wtime)
+            use_wtime=args.use_wtime, content_filter_options=content_filter_options)
 
 
 class ROSTopicHz(object):
@@ -244,13 +257,15 @@ def print_hz(self, topic=None):
         return
 
 
-def _rostopic_hz(node, topic, window_size=DEFAULT_WINDOW_SIZE, filter_expr=None, use_wtime=False):
+def _rostopic_hz(node, topic, window_size=DEFAULT_WINDOW_SIZE, filter_expr=None,
+                 use_wtime=False, content_filter_options=None):
     """
     Periodically print the publishing rate of a topic to console until shutdown.
 
     :param topic: topic name, ``list`` of ``str``
     :param window_size: number of messages to average over, -1 for infinite, ``int``
     :param filter_expr: Python filter expression that is called with m, the message instance
+    :param content_filter_options: DDS content filter options, ``ContentFilterOptions``
     """
     # pause hz until topic is published
     msg_class = get_msg_class(
@@ -266,7 +281,8 @@ def _rostopic_hz(node, topic, window_size=DEFAULT_WINDOW_SIZE, filter_expr=None,
         topic,
         functools.partial(rt.callback_hz, topic=topic),
         qos_profile_sensor_data,
-        raw=filter_expr is None)
+        raw=filter_expr is None,
+        content_filter_options=content_filter_options)
 
     while rclpy.ok():
         rclpy.spin_once(node)