diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index bf7e36c4c0c0a..58f73ca3836c2 100644 --- 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 index fe9615614de81..aa46c0e2fc7ef 100644 --- 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 ``` 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 -m +``` + +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 new file mode 100644 index 0000000000000..fb064372796ae Binary files /dev/null and b/planning/planning_debug_tools/image/processing_time_checker.png differ diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index deadcd54fffdd..14db1277cc02a 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -6,6 +6,7 @@ The planning_debug_tools package Takamasa Horibe Taiki Tanaka + Takayuki Murooka Apache License 2.0 Takamasa Horibe diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py new file mode 100755 index 0000000000000..3b80ce4e5588f --- /dev/null +++ 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()