feat(processing_time_checker): add a script to display processing time (

#5375) * feat(processing_time_checker): add script to visualize processing time Signed-off-by: Takamasa Horibe <[email protected]> * add maintainer Signed-off-by: Takamasa Horibe <[email protected]> * update readme Signed-off-by: Takamasa Horibe <[email protected]> * add empty case Signed-off-by: Takamasa Horibe <[email protected]> * fix format Signed-off-by: Takamasa Horibe <[email protected]> --------- Signed-off-by: Takamasa Horibe <[email protected]>
autowarefoundation · Oct 23, 2023 · 911a272 · 911a272
1 parent eca2d25
commit 911a272
Show file tree

Hide file tree

Showing 5 changed files with 129 additions and 0 deletions.
diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt
@@ -48,6 +48,7 @@ ament_auto_package(
 )
 
 install(PROGRAMS
+  scripts/processing_time_checker.py
   scripts/trajectory_visualizer.py
   scripts/closest_velocity_checker.py
   scripts/perception_replayer/perception_reproducer.py

diff --git a/planning/planning_debug_tools/README.md b/planning/planning_debug_tools/README.md
@@ -4,6 +4,8 @@ This package contains several planning-related debug tools.
 
 - **Trajectory analyzer**: visualizes the information (speed, curvature, yaw, etc) along the trajectory
 - **Closest velocity checker**: prints the velocity information indicated by each modules
+- **Perception reproducer**: generates detected objects from rosbag data in planning simulator environment
+- **processing time checker**: displays processing_time of modules on the terminal
 
 ## Trajectory analyzer
 
@@ -241,3 +243,24 @@ ros2 run planning_debug_tools perception_replayer.py -b <dir-to-bag-files>
 ```
 
 Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively.
+
+## Processing time checker
+
+The purpose of the Processing Time Subscriber is to monitor and visualize the processing times of various ROS 2 topics in a system. By providing a real-time terminal-based visualization, users can easily confirm the processing time performance as in the picture below.
+
+![processing_time_checker](image/processing_time_checker.png)
+
+You can run the program by the following command.
+
+```bash
+ros2 run planning_debug_tools processing_time_checker.py -f <update-hz> -m <max-bar-time>
+```
+
+This program subscribes to ROS 2 topics that have a suffix of `processing_time_ms`.
+
+The program allows users to customize two parameters via command-line arguments:
+
+- --max_display_time (or -m): This sets the maximum display time in milliseconds. The default value is 150ms.
+- --display_frequency (or -f): This sets the frequency at which the terminal UI updates. The default value is 5Hz.
+
+By adjusting these parameters, users can tailor the display to their specific monitoring needs.
diff --git a/planning/planning_debug_tools/image/processing_time_checker.png b/planning/planning_debug_tools/image/processing_time_checker.png
diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml
@@ -6,6 +6,7 @@
   <description>The planning_debug_tools package</description>
   <maintainer email="[email protected]">Takamasa Horibe</maintainer>
   <maintainer email="[email protected]">Taiki Tanaka</maintainer>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Takamasa Horibe</author>

diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py
@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import argparse
+import os
+import sys
+
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import Float64Stamped
+
+
+class ProcessingTimeSubscriber(Node):
+    def __init__(self, max_display_time=150, display_frequency=5.0):
+        super().__init__("processing_time_subscriber")
+        self.data_map = {}  # {topic_name: data_value}
+        self.max_display_time = max_display_time
+        self.get_topic_list()
+
+        # Timer to print data at given frequency
+        self.create_timer(1.0 / display_frequency, self.print_data)
+
+    def get_topic_list(self):
+        # Get list of existing topics in the system
+        topic_list = self.get_topic_names_and_types()
+
+        # Subscribe to topics with 'processing_time_ms' suffix
+        for topic, types in topic_list:
+            if topic.endswith("processing_time_ms"):
+                self.create_subscription(
+                    Float64Stamped, topic, lambda msg, topic=topic: self.callback(msg, topic), 1
+                )
+                self.get_logger().info(f"Subscribed to {topic} | Type: {types}")
+
+    def callback(self, msg, topic):
+        self.data_map[topic] = msg.data
+
+    def print_data(self):
+        # Clear terminal
+        os.system("cls" if os.name == "nt" else "clear")
+
+        if not self.data_map:
+            print("No topics available.")
+            return
+
+        # Get the maximum topic name length for alignment
+        max_topic_length = max(map(len, self.data_map.keys()))
+
+        # Generate the header based on the max_display_time
+        header_str = "topics".ljust(max_topic_length) + ":"
+        for i in range(0, self.max_display_time + 1, 20):
+            header_str += f" {i}ms".ljust(20)
+
+        # Print the header
+        print(header_str)
+        print("-" * len(header_str))
+
+        # Print each topic's data
+        for topic, data in self.data_map.items():
+            # Round the data to the third decimal place for display
+            data_rounded = round(data, 3)
+            # Calculate the number of bars to be displayed (clamped to max_display_time)
+            num_bars = int(min(data, self.max_display_time - 1)) + 1
+            print(f"{topic.ljust(max_topic_length)}: {'|' * num_bars} ({data_rounded}ms)")
+
+
+def main(args=None):
+    # Get the command line arguments before argparse processes them
+    cmd_args = sys.argv[1:]
+
+    parser = argparse.ArgumentParser(description="Processing Time Subscriber Parameters")
+    parser.add_argument(
+        "-m", "--max_display_time", type=int, default=150, help="Maximum display time in ms"
+    )
+    parser.add_argument(
+        "-f", "--display_frequency", type=float, default=5.0, help="Display frequency in Hz"
+    )
+    args = parser.parse_args()
+
+    rclpy.init(args=cmd_args)  # Use the original command line arguments here
+    subscriber = ProcessingTimeSubscriber(
+        max_display_time=args.max_display_time, display_frequency=args.display_frequency
+    )
+    rclpy.spin(subscriber)
+    subscriber.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()