diff --git a/planning/autoware_planning_data_analyzer/CMakeLists.txt b/planning/autoware_planning_data_analyzer/CMakeLists.txt index cf6461779..bbfcc6706 100644 --- a/planning/autoware_planning_data_analyzer/CMakeLists.txt +++ b/planning/autoware_planning_data_analyzer/CMakeLists.txt @@ -2,15 +2,12 @@ cmake_minimum_required(VERSION 3.14) project(autoware_planning_data_analyzer) find_package(autoware_cmake REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/buffer.cpp src/base_evaluator.cpp src/open_loop_evaluator.cpp - src/or_event_extractor.cpp - src/or_scene_evaluator.cpp src/metrics/deviation_metrics.cpp src/metrics/epdms_aggregation.cpp src/metrics/ego_progress.cpp @@ -29,7 +26,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/path_utils.cpp src/autoware_planning_data_analyzer_node.cpp ) -ament_target_dependencies(${PROJECT_NAME} tf2_geometry_msgs) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::planning_data_analyzer::AutowarePlanningDataAnalyzerNode" @@ -67,5 +63,4 @@ ament_auto_package( INSTALL_TO_SHARE config launch - scripts ) diff --git a/planning/autoware_planning_data_analyzer/README.md b/planning/autoware_planning_data_analyzer/README.md index d69329c0b..873a08cef 100644 --- a/planning/autoware_planning_data_analyzer/README.md +++ b/planning/autoware_planning_data_analyzer/README.md @@ -4,11 +4,6 @@ This package provides offline evaluation tools for trajectory planning performance analysis from recorded rosbag data. -## Evaluation Modes - -1. **Open Loop**: Evaluate prediction accuracy against ground truth trajectories -2. **OR Scene**: Evaluate override regression scenarios (LIVE vs HISTORICAL) - ## Open-Loop Evaluation Open-loop evaluation computes both trajectory error metrics and planning-quality subscores. @@ -63,31 +58,6 @@ ros2 run autoware_planning_data_analyzer autoware_planning_data_analyzer_node -- -p json_output_path:=~/results.json ``` -### OR Scene Evaluation - -```sh -# Using the unified script -./install/autoware_planning_data_analyzer/share/autoware_planning_data_analyzer/scripts/run_evaluation.sh \ - -b ~/result_bag.mcap \ - -m or_scene \ - -t /planning/diffusion_planner/trajectory \ - -o ~/results \ - --live \ - --viz -``` - -### Multi-Model Pipeline - -```sh -# Evaluate multiple models from DLR results -./install/autoware_planning_data_analyzer/share/autoware_planning_data_analyzer/scripts/multi_model_pipeline.sh \ - --scenario ~/dataset/scenario.yaml \ - --trajectory /planning/model_a/trajectory \ - --trajectory /planning/model_b/trajectory \ - --output ~/comparison \ - --viz -``` - ## Output Files - **`evaluation_result.json`** - Detailed metrics (ADE, FDE, TTC, etc.) and summary statistics @@ -96,28 +66,17 @@ ros2 run autoware_planning_data_analyzer autoware_planning_data_analyzer_node -- ## Parameters -Key parameters in `config/offline_evaluation.param.yaml`: +Key parameters in `config/planning_data_analyzer.param.yaml`: -- `evaluation.mode`: Evaluation mode (`open_loop` or `or_scene`) +- `evaluation.mode`: Evaluation mode (`open_loop`) - `evaluation_interval_ms`: Sampling interval (default: 100ms) - `sync_tolerance_ms`: Time synchronization tolerance (default: 100ms) - `trajectory_topic`: Trajectory topic to evaluate -- `or_scene_evaluation.*`: OR scene specific settings - -### Utility Scripts - -Located in `scripts/` directory: - -- **Bag Processing**: `merge_bags.py`, `rename_bag_topic.py`, `add_prefix_to_bag.py` -- **Ground Truth**: `add_gt_trajectory_to_bag.py` -- **OR Analysis**: `detect_or_and_route.py`, `evaluate_or_segments.py` -- **Visualization**: `generate_or_visualization.py` -- **Comparison**: `compare_live_historical.py` +- `open_loop.*`: Open-loop specific settings --- ## For More Information -- Offline evaluation scripts: [scripts/](scripts/) - Configuration: [config/planning_data_analyzer.param.yaml](config/planning_data_analyzer.param.yaml) - Launch files: [launch/](launch/) diff --git a/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml b/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml index 39c8b0209..9bdd75b27 100644 --- a/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml +++ b/planning/autoware_planning_data_analyzer/config/planning_data_analyzer.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: # Evaluation Mode - # Options: "open_loop" or "or_scene" + # Options: "open_loop" evaluation: mode: "open_loop" @@ -60,34 +60,3 @@ lane_keep: max_lateral_deviation: 0.5 # Maximum absolute lateral deviation allowed for LK [m] max_continuous_violation_time: 2.0 # Continuous over-threshold duration allowed for LK [s] - - # OR Scene Evaluation Configuration (only used when mode="or_scene") - or_scene_evaluation: - # Time window configuration (symmetric around OR event) - time_window_sec: 0.5 # Time window on each side of OR event [OR - 0.5s, OR + 0.5s] - - # Trajectory source selection - evaluate_live_trajectories: true # true = evaluate result_bag (DLR output with LIVE trajectories) - # false = evaluate input_bag (HISTORICAL recorded trajectories) - input_bag_path: "" # Path to input bag (only used if evaluate_live_trajectories=false) - - # Topics to monitor - control_mode_topic: "/vehicle/status/control_mode" # Topic for OR detection (ControlModeReport) - - # OR event extraction (Stage 1) - skip_or_extraction: false # Set true if or_events.json already exists - or_events_input_path: "" # Path to pre-extracted or_events.json (optional) - or_events_output_path: "~/or_events.json" # Where to save extracted OR events - - # Success criteria (optional - disabled by default) - success_criteria: - enabled: false # Set true to enforce thresholds - max_ade: 1.0 # Maximum allowed ADE (meters) - max_fde: 1.5 # Maximum allowed FDE (meters) - max_lateral_deviation: 0.5 # Maximum absolute lateral deviation in the vehicle frame (meters) - min_ttc: 3.0 # Minimum required TTC (seconds) - - # Debug visualization - enable_debug_visualization: false # Generate debug images showing predicted vs GT trajectories - debug_output_dir: "~/or_scene_debug_images" # Directory for debug visualization images - map_path: "" # Optional: path to lanelet2_map.osm for lane visualization diff --git a/planning/autoware_planning_data_analyzer/scripts/add_gt_trajectory_to_bag.py b/planning/autoware_planning_data_analyzer/scripts/add_gt_trajectory_to_bag.py deleted file mode 100755 index 4e8f31f35..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/add_gt_trajectory_to_bag.py +++ /dev/null @@ -1,263 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable - -"""Add ground truth trajectory topic to a bag.""" - -import argparse - -from autoware_planning_msgs.msg import Trajectory -from autoware_planning_msgs.msg import TrajectoryPoint -from builtin_interfaces.msg import Duration -from geometry_msgs.msg import Pose -from nav_msgs.msg import Odometry -import numpy as np -import rclpy -from rclpy.serialization import deserialize_message -from rclpy.serialization import serialize_message -from rosbag2_py import ConverterOptions -from rosbag2_py import Reindexer -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions -from rosbag2_py import TopicMetadata - - -def quat_slerp(q1, q2, t): - """SLERP interpolation between two quaternions.""" - # Normalize - q1 = np.array([q1.x, q1.y, q1.z, q1.w]) - q2 = np.array([q2.x, q2.y, q2.z, q2.w]) - - q1 = q1 / np.linalg.norm(q1) - q2 = q2 / np.linalg.norm(q2) - - dot = np.dot(q1, q2) - - # If negative, reverse one quaternion - if dot < 0.0: - q2 = -q2 - dot = -dot - - # If very close, use linear interpolation - if dot > 0.9995: - result = q1 + t * (q2 - q1) - return result / np.linalg.norm(result) - - # SLERP - theta = np.arccos(np.clip(dot, -1.0, 1.0)) - result = (np.sin((1 - t) * theta) * q1 + np.sin(t * theta) * q2) / np.sin(theta) - return result - - -def interpolate_pose(odom1, odom2, t): - """Interpolate pose between two odometry messages.""" - pose = Pose() - - # Linear interpolation for position - pose.position.x = odom1.pose.pose.position.x + t * ( - odom2.pose.pose.position.x - odom1.pose.pose.position.x - ) - pose.position.y = odom1.pose.pose.position.y + t * ( - odom2.pose.pose.position.y - odom1.pose.pose.position.y - ) - pose.position.z = odom1.pose.pose.position.z + t * ( - odom2.pose.pose.position.z - odom1.pose.pose.position.z - ) - - # SLERP for orientation - q_interp = quat_slerp(odom1.pose.pose.orientation, odom2.pose.pose.orientation, t) - pose.orientation.x = q_interp[0] - pose.orientation.y = q_interp[1] - pose.orientation.z = q_interp[2] - pose.orientation.w = q_interp[3] - - return pose - - -def generate_gt_trajectory( - all_odometry, current_index, current_time_ns, horizon_sec, resolution_sec -): - """Generate GT trajectory from future kinematic states.""" - trajectory = Trajectory() - - # Header - trajectory.header.frame_id = "map" - trajectory.header.stamp.sec = int(current_time_ns // 1e9) - trajectory.header.stamp.nanosec = int(current_time_ns % 1e9) - - # Generate points by looking ahead - horizon_ns = int(horizon_sec * 1e9) - resolution_ns = int(resolution_sec * 1e9) - - for dt_ns in range(0, horizon_ns + 1, resolution_ns): - target_time_ns = current_time_ns + dt_ns - - # Find bracketing odometry messages - idx1 = None - idx2 = None - - for i in range(current_index, len(all_odometry)): - odom_time_ns = all_odometry[i]["timestamp"] - - if odom_time_ns <= target_time_ns: - idx1 = i - elif odom_time_ns > target_time_ns: - idx2 = i - break - - if idx1 is None: - break # No data - - # Create trajectory point - point = TrajectoryPoint() - - if idx2 is None or idx1 == idx2: - # Use exact point or last available - point.pose = all_odometry[idx1]["odom"].pose.pose - else: - # Interpolate - time1 = all_odometry[idx1]["timestamp"] - time2 = all_odometry[idx2]["timestamp"] - t = (target_time_ns - time1) / (time2 - time1) - point.pose = interpolate_pose(all_odometry[idx1]["odom"], all_odometry[idx2]["odom"], t) - - # Time from start - point.time_from_start = Duration() - point.time_from_start.sec = int(dt_ns // 1e9) - point.time_from_start.nanosec = int(dt_ns % 1e9) - - trajectory.points.append(point) - - return trajectory if len(trajectory.points) > 0 else None - - -def add_gt_trajectories(input_bag, output_bag, horizon_sec=8.0, resolution_sec=0.1): - """Add GT trajectory topic to bag.""" - print("Adding GT trajectories to bag:") - print(f" Input: {input_bag}") - print(f" Output: {output_bag}") - print(f" Horizon: {horizon_sec}s") - print(f" Resolution: {resolution_sec}s") - print("") - - # Step 1: Load all kinematic states - # Use empty storage_id to let rosbag2 auto-detect format - print("Step 1: Loading all kinematic states...") - storage_options_read = StorageOptions(uri=str(input_bag), storage_id="") - converter_options = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - - reader1 = SequentialReader() - reader1.open(storage_options_read, converter_options) - - all_odometry = [] - while reader1.has_next(): - topic, data, timestamp = reader1.read_next() - if topic == "/localization/kinematic_state": - odom = deserialize_message(data, Odometry) - all_odometry.append({"timestamp": timestamp, "odom": odom}) - - del reader1 - print(f" Loaded {len(all_odometry)} kinematic states") - - # Step 2: Process bag and add GT trajectories - print("\nStep 2: Processing bag and generating GT trajectories...") - - reader2 = SequentialReader() - reader2.open(storage_options_read, converter_options) - - storage_options_write = StorageOptions(uri=str(output_bag), storage_id="mcap") - writer = SequentialWriter() - writer.open(storage_options_write, converter_options) - - # Create all existing topics - for topic_metadata in reader2.get_all_topics_and_types(): - writer.create_topic(topic_metadata) - - # Add GT trajectory topic - gt_topic = TopicMetadata( - name="/ground_truth/trajectory", - type="autoware_planning_msgs/msg/Trajectory", - serialization_format="cdr", - ) - writer.create_topic(gt_topic) - print("Created topic: /ground_truth/trajectory") - - # Process messages - message_count = 0 - gt_count = 0 - odom_index = 0 - - while reader2.has_next(): - topic, data, timestamp = reader2.read_next() - - # Write original message - writer.write(topic, data, timestamp) - message_count += 1 - - # Generate GT trajectory for each kinematic_state - if topic == "/localization/kinematic_state": - gt_traj = generate_gt_trajectory( - all_odometry, odom_index, timestamp, horizon_sec, resolution_sec - ) - - if gt_traj: - gt_data = serialize_message(gt_traj) - writer.write("/ground_truth/trajectory", gt_data, timestamp) - gt_count += 1 - - odom_index += 1 - - if message_count % 10000 == 0: - print(f" Processed {message_count} messages, generated {gt_count} GT trajectories...") - - print("\nCopy complete:") - print(f" Total messages copied: {message_count}") - print(f" GT trajectories generated: {gt_count}") - - del writer - del reader2 - - # Reindex - print("\nReindexing...") - Reindexer().reindex(storage_options_write) - print("Done!") - - -def main(): - parser = argparse.ArgumentParser(description="Add ground truth trajectory topic to a rosbag") - parser.add_argument("--input", required=True, help="Input bag directory or mcap file") - parser.add_argument("--output", required=True, help="Output bag directory") - parser.add_argument( - "--horizon", type=float, default=8.0, help="Look-ahead horizon in seconds (default: 8.0)" - ) - parser.add_argument( - "--resolution", type=float, default=0.1, help="Sample resolution in seconds (default: 0.1)" - ) - - args = parser.parse_args() - - rclpy.init() - - add_gt_trajectories(args.input, args.output, args.horizon, args.resolution) - - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/add_prefix_to_bag.py b/planning/autoware_planning_data_analyzer/scripts/add_prefix_to_bag.py deleted file mode 100755 index 7b32b4849..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/add_prefix_to_bag.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable - -"""Add prefixed trajectory topic to MCAP bag (in-place modification).""" - -import argparse -from pathlib import Path -import shutil -import sys -import tempfile - -from rosbag2_py import ConverterOptions -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions -from rosbag2_py import TopicMetadata - - -def add_prefixed_topic(bag_path: str, prefix: str, source_topic: str): - """Add prefixed copy of source topic to bag.""" - # Replace dots with underscores for ROS compliance (v2.0 -> v2_0) - prefix_safe = prefix.replace(".", "_") - prefixed_topic = f"/{prefix_safe}{source_topic}" - - print(f"Adding prefixed topic to: {bag_path}") - print(f" Source: {source_topic}") - print(f" Prefixed: {prefixed_topic}") - - # Step 1: Read all messages - reader = SequentialReader() - reader.open(StorageOptions(uri=bag_path, storage_id=""), ConverterOptions("cdr", "cdr")) - - all_topics = reader.get_all_topics_and_types() - traj_type = None - for topic_meta in all_topics: - if topic_meta.name == source_topic: - traj_type = topic_meta.type - break - - if not traj_type: - print(f"ERROR: Topic {source_topic} not found in bag") - sys.exit(1) - - traj_messages = [] - all_messages = [] - - while reader.has_next(): - topic, data, timestamp = reader.read_next() - all_messages.append((topic, data, timestamp)) - if topic == source_topic: - traj_messages.append((data, timestamp)) - - del reader - - if len(traj_messages) == 0: - print(f"WARNING: No messages found at {source_topic}") - sys.exit(0) - - print(f" Found {len(traj_messages)} trajectory messages") - - # Step 2: Write new bag to temp location - temp_bag = tempfile.mkdtemp(prefix="bag_prefix_") - shutil.rmtree(temp_bag) # Remove the directory, we just need the unique name - - try: - writer = SequentialWriter() - writer.open(StorageOptions(uri=temp_bag, storage_id="mcap"), ConverterOptions("cdr", "cdr")) - - # Create all original topics - for topic_meta in all_topics: - writer.create_topic(topic_meta) - - # Create prefixed topic - writer.create_topic( - TopicMetadata(name=prefixed_topic, type=traj_type, serialization_format="cdr") - ) - - # Write all original messages - for topic, data, timestamp in all_messages: - writer.write(topic, data, timestamp) - - # Write prefixed trajectory messages - for data, timestamp in traj_messages: - writer.write(prefixed_topic, data, timestamp) - - del writer - - # Skip Python reindexing - preserves message schemas for Lichtblick - # Reindexer().reindex(StorageOptions(uri=temp_bag, storage_id="mcap")) - - # Replace original - shutil.rmtree(bag_path) - shutil.move(temp_bag, bag_path) - - print(f" ✓ Added {len(traj_messages)} messages to {prefixed_topic}") - - except Exception as e: - if Path(temp_bag).exists(): - shutil.rmtree(temp_bag) - raise e - - -def main(): - parser = argparse.ArgumentParser(description="Add prefixed trajectory topic to bag") - parser.add_argument("--bag", required=True, help="Path to result_bag directory") - parser.add_argument("--prefix", required=True, help="Topic prefix (e.g., v2.0)") - parser.add_argument( - "--source-topic", - default="/planning/trajectory_generator/diffusion_planner_node/output/trajectory", - help="Source topic to copy", - ) - - args = parser.parse_args() - - if not Path(args.bag).exists(): - print(f"ERROR: Bag not found: {args.bag}") - sys.exit(1) - - add_prefixed_topic(args.bag, args.prefix, args.source_topic) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/cut_or_segment_with_route.py b/planning/autoware_planning_data_analyzer/scripts/cut_or_segment_with_route.py deleted file mode 100755 index 86406d054..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/cut_or_segment_with_route.py +++ /dev/null @@ -1,185 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable -""" -Cut rosbag segment for OR event time window and inject route message. - -This ensures each segment is self-contained with route available for DLR. -""" - -import argparse -from pathlib import Path -import sys - -from rosbag2_py import ConverterOptions -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions - - -def cut_segment_with_route( - input_bag: str, - output_bag: str, - start_ns: int, - end_ns: int, - route_msg_data: bytes, - route_topic: str = "/planning/mission_planning/route", -): - """ - Cut time segment and inject route message at segment start. - - Args: - input_bag: Source bag path - output_bag: Destination bag path - start_ns: Segment start timestamp (nanoseconds) - end_ns: Segment end timestamp (nanoseconds) - route_msg_data: Serialized route message to inject - route_topic: Route topic name - """ - print(f"Cutting segment: [{start_ns / 1e9:.3f}s, {end_ns / 1e9:.3f}s]") - print(f" Input: {input_bag}") - print(f" Output: {output_bag}") - - # Open reader - reader = SequentialReader() - reader_storage = StorageOptions(uri=str(input_bag), storage_id="") - reader_converter = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - reader.open(reader_storage, reader_converter) - - # Open writer - writer = SequentialWriter() - writer_storage = StorageOptions(uri=str(output_bag), storage_id="mcap") - writer_converter = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - writer.open(writer_storage, writer_converter) - - # Create all topics from input bag - for topic_metadata in reader.get_all_topics_and_types(): - writer.create_topic(topic_metadata) - - # CRITICAL: Inject route message at segment start - writer.write(route_topic, route_msg_data, start_ns) - print(f" ✓ Injected route at t={start_ns / 1e9:.3f}s") - - # Copy messages in time window - found_topics = {route_topic} # Route already added - message_count = 1 # Route counts - skipped_route_count = 0 - - while reader.has_next(): - topic, data, timestamp = reader.read_next() - - if start_ns <= timestamp <= end_ns: - # Skip route if it appears in the segment (we already injected it at start) - if topic == route_topic: - skipped_route_count += 1 - continue - - writer.write(topic, data, timestamp) - message_count += 1 - found_topics.add(topic) - - if message_count % 1000 == 0: - print(f" Copied {message_count} messages...", end="\r") - - print(f" Copied {message_count} messages total") - if skipped_route_count > 0: - print(f" Skipped {skipped_route_count} duplicate route messages") - - # Check for critical topics - critical_topics = [ - "/planning/mission_planning/route", - "/localization/kinematic_state", - "/perception/object_recognition/tracking/objects", - "/vehicle/status/control_mode", - "/vehicle/status/turn_indicators_status", - ] - - missing = [] - for topic in critical_topics: - if topic not in found_topics: - missing.append(topic) - - if missing: - print(f" WARNING: Segment missing topics: {missing}") - - print(f" ✓ Segment duration: {(end_ns - start_ns) / 1e9:.2f}s") - print(f" ✓ Topics found: {len(found_topics)}") - - return { - "message_count": message_count, - "duration_sec": (end_ns - start_ns) / 1e9, - "topics_found": len(found_topics), - "has_route": route_topic in found_topics, - } - - -def main(): - parser = argparse.ArgumentParser(description="Cut rosbag segment with route message injection") - parser.add_argument("--input", "-i", required=True, help="Input bag path") - parser.add_argument("--output", "-o", required=True, help="Output bag path") - parser.add_argument("--start", type=int, required=True, help="Start timestamp (nanoseconds)") - parser.add_argument("--end", type=int, required=True, help="End timestamp (nanoseconds)") - parser.add_argument( - "--route-data", required=True, help="File containing serialized route message" - ) - parser.add_argument( - "--route-topic", - default="/planning/mission_planning/route", - help="Route topic name", - ) - - args = parser.parse_args() - - # Validate inputs - if not Path(args.input).exists(): - print(f"ERROR: Input bag not found: {args.input}") - sys.exit(1) - - if not Path(args.route_data).exists(): - print(f"ERROR: Route data file not found: {args.route_data}") - sys.exit(1) - - # Remove output if it exists (allow overwriting) - if Path(args.output).exists(): - import shutil - - shutil.rmtree(args.output) - print(f"Removed existing output: {args.output}") - - # Load route message data - with open(args.route_data, "rb") as f: - route_msg_data = f.read() - - print(f"Loaded route message: {len(route_msg_data)} bytes") - - # Cut segment - try: - _ = cut_segment_with_route( - args.input, args.output, args.start, args.end, route_msg_data, args.route_topic - ) - print("\n✓ Segment cut successfully") - return 0 - except Exception as e: - print(f"\nERROR: Failed to cut segment: {e}") - return 1 - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/planning/autoware_planning_data_analyzer/scripts/detect_or_and_route.py b/planning/autoware_planning_data_analyzer/scripts/detect_or_and_route.py deleted file mode 100755 index 2ed4592ee..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/detect_or_and_route.py +++ /dev/null @@ -1,208 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable - -""" -Detect OR (Override) events and extract route message from rosbag. - -Outputs JSON with OR event timestamps and saves route message for injection into segments. -""" - -import argparse -import json -from pathlib import Path -import sys - -from rclpy.serialization import deserialize_message -from rosbag2_py import ConverterOptions -from rosbag2_py import SequentialReader -from rosbag2_py import StorageOptions - -try: - from autoware_vehicle_msgs.msg import ControlModeReport -except ImportError: - print("ERROR: Required ROS message types not found.") - print("Make sure to source Autoware workspace first:") - print(" source ~/pilot-auto/install/setup.bash") - sys.exit(1) - - -def detect_or_events_and_extract_route(bag_path: str, before_sec: float, after_sec: float): - """ - Detect OR events and extract route message. - - Args: - bag_path: Input bag directory or file - before_sec: Seconds before OR to include in segment - after_sec: Seconds after OR to include in segment - - Returns: - dict with 'or_events' list and 'route_info' dict - """ - reader = SequentialReader() - storage_options = StorageOptions(uri=str(bag_path), storage_id="") - converter_options = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - - reader.open(storage_options, converter_options) - - or_events = [] - route_msg_data = None - route_timestamp = None - prev_mode = None - or_count = 0 - - # Get bag start/end times - metadata = reader.get_metadata() - bag_start_ns = metadata.starting_time.nanoseconds - bag_duration_ns = metadata.duration.nanoseconds - bag_end_ns = bag_start_ns + bag_duration_ns - - print(f"Scanning bag: {bag_path}") - print(f" Duration: {bag_duration_ns / 1e9:.2f}s") - print(f" Start: {bag_start_ns}") - print(f" End: {bag_end_ns}") - print("") - - while reader.has_next(): - topic, data, timestamp = reader.read_next() - - # Detect OR events - if topic == "/vehicle/status/control_mode": - try: - msg = deserialize_message(data, ControlModeReport) - # AUTONOMOUS (1) → MANUAL (4) = Override - if prev_mode == 1 and msg.mode == 4: - or_count += 1 - - # Calculate segment boundaries - seg_start_ns = max(timestamp - int(before_sec * 1e9), bag_start_ns) - seg_end_ns = min(timestamp + int(after_sec * 1e9), bag_end_ns) - - or_events.append( - { - "event_id": or_count - 1, - "or_timestamp_ns": timestamp, - "or_timestamp_sec": timestamp / 1e9, - "segment_start_ns": seg_start_ns, - "segment_end_ns": seg_end_ns, - "segment_start_sec": seg_start_ns / 1e9, - "segment_end_sec": seg_end_ns / 1e9, - "segment_duration_sec": (seg_end_ns - seg_start_ns) / 1e9, - } - ) - - print(f"OR Event #{or_count} detected at t={timestamp / 1e9:.3f}s") - print( - f" Segment: [{seg_start_ns / 1e9:.3f}s, {seg_end_ns / 1e9:.3f}s] " - f"(duration: {(seg_end_ns - seg_start_ns) / 1e9:.2f}s)" - ) - - prev_mode = msg.mode - except Exception as e: - print(f"Warning: Failed to deserialize control_mode: {e}") - - # Extract route (first occurrence) - if topic == "/planning/mission_planning/route" and route_msg_data is None: - route_msg_data = data - route_timestamp = timestamp - print(f"✓ Route message found at t={timestamp / 1e9:.3f}s") - - if not or_events: - print("ERROR: No OR events found in bag!") - sys.exit(1) - - if route_msg_data is None: - print("ERROR: No route message found in bag!") - print("Route topic: /planning/mission_planning/route") - sys.exit(1) - - print(f"\nTotal OR events detected: {len(or_events)}") - - return { - "or_events": or_events, - "route_info": { - "original_timestamp_ns": route_timestamp, - "original_timestamp_sec": route_timestamp / 1e9, - "data_size_bytes": len(route_msg_data), - }, - "route_msg_data": route_msg_data, # Binary data for injection - "bag_info": { - "start_ns": bag_start_ns, - "end_ns": bag_end_ns, - "duration_sec": bag_duration_ns / 1e9, - }, - } - - -def main(): - parser = argparse.ArgumentParser( - description="Detect OR events and extract route message from rosbag" - ) - parser.add_argument("--input", "-i", required=True, help="Input bag path") - parser.add_argument("--output", "-o", required=True, help="Output JSON file for OR events") - parser.add_argument( - "--route-output", - required=True, - help="Output file to save route message binary data", - ) - parser.add_argument( - "--before", - type=float, - default=5.0, - help="Seconds before OR to include (default: 5.0)", - ) - parser.add_argument( - "--after", - type=float, - default=10.0, - help="Seconds after OR to include (default: 10.0)", - ) - - args = parser.parse_args() - - # Validate input - if not Path(args.input).exists(): - print(f"ERROR: Input bag not found: {args.input}") - sys.exit(1) - - # Detect OR events and extract route - result = detect_or_events_and_extract_route(args.input, args.before, args.after) - - # Save OR events JSON (without binary route data) - output_json = { - "or_events": result["or_events"], - "route_info": result["route_info"], - "bag_info": result["bag_info"], - "parameters": {"before_sec": args.before, "after_sec": args.after}, - } - - with open(args.output, "w") as f: - json.dump(output_json, f, indent=2) - - print(f"\n✓ OR events saved to: {args.output}") - - # Save route message binary data - with open(args.route_output, "wb") as f: - f.write(result["route_msg_data"]) - - print(f"✓ Route message saved to: {args.route_output}") - print(f" Size: {len(result['route_msg_data'])} bytes") - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/evaluate_all_live_trajectories.py b/planning/autoware_planning_data_analyzer/scripts/evaluate_all_live_trajectories.py deleted file mode 100755 index 26ea720c4..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/evaluate_all_live_trajectories.py +++ /dev/null @@ -1,297 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable -"""Evaluate all LIVE trajectory topics in a bag simultaneously.""" - -import argparse -import json -from pathlib import Path -import sys - -import rclpy -from rosbag2_py import ConverterOptions -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions - - -def detect_live_trajectory_topics(bag_path: str): - """Detect all LIVE trajectory topics with prefixes.""" - reader = SequentialReader() - reader.open(StorageOptions(uri=bag_path, storage_id=""), ConverterOptions("cdr", "cdr")) - - live_topics = [] - for meta in reader.get_all_topics_and_types(): - # Look for trajectory topics with prefixes (e.g., /v2.0/planning/...) - if "trajectory_generator" in meta.name and "/output/trajectory" in meta.name: - # Extract prefix (first component after /) - parts = meta.name.strip("/").split("/") - if parts[0] not in ["planning", "control", "localization", "perception"]: - # This is a prefixed topic - prefix = parts[0] - # Convert underscores back to dots for display (v2_0 -> v2.0) - prefix_display = prefix.replace("_", ".") - live_topics.append( - { - "topic": meta.name, - "prefix": prefix, - "prefix_display": prefix_display, - "type": meta.type, - } - ) - - del reader - return live_topics - - -def evaluate_all_trajectories( - bag_path: str, - input_bag_path: str, - map_path: str, - output_bag_path: str, - json_output_path: str = None, - time_window: float = 0.5, -): - """ - Evaluate all LIVE trajectories in a single pass. - - Args: - bag_path: Path to bag with LIVE trajectories (result bag) - input_bag_path: Path to original input bag (for OR event extraction) - map_path: Path to lanelet2 map file - output_bag_path: Output bag with metrics injected - json_output_path: Optional JSON output file - """ - print(f"Evaluating all LIVE trajectories in: {bag_path}") - print(f"Input bag (for OR extraction): {input_bag_path}") - - # Step 1: Detect LIVE trajectory topics - live_topics = detect_live_trajectory_topics(bag_path) - - if len(live_topics) == 0: - print("ERROR: No LIVE trajectory topics found!") - sys.exit(1) - - print(f"\nDetected {len(live_topics)} LIVE trajectory topics:") - for lt in live_topics: - print(f" {lt['topic']} (prefix: {lt['prefix']})") - - # Step 2: Run OR scene evaluation for each LIVE trajectory - all_results = {} - - for lt in live_topics: - prefix = lt["prefix"] - traj_topic = lt["topic"] - - print(f"\nEvaluating {prefix}...") - - # Clean up old metric bag if it exists - import shutil - - metric_bag_dir = f"/tmp/eval_{prefix}_metrics.bag" - if Path(metric_bag_dir).exists(): - shutil.rmtree(metric_bag_dir) - - # Run autoware_planning_data_analyzer_node for this trajectory - import subprocess - - cmd = [ - "ros2", - "run", - "autoware_planning_data_analyzer", - "autoware_planning_data_analyzer_node", - "--ros-args", - "-p", - f"bag_path:={bag_path}", - "-p", - "evaluation.mode:=or_scene", - "-p", - f"trajectory_topic:={traj_topic}", - "-p", - f"or_scene_evaluation.input_bag_path:={input_bag_path}", - "-p", - f"or_scene_evaluation.map_path:={map_path}", - "-p", - f"or_scene_evaluation.time_window_sec:={time_window}", - "-p", - "or_scene_evaluation.enable_debug_visualization:=false", - "-p", - f"evaluation_output_bag_path:=/tmp/eval_{prefix}_metrics.bag", - ] - - result = subprocess.run(cmd, capture_output=True, text=True) - - if result.returncode != 0: - print(f" WARNING: Evaluation failed for {prefix}") - print(f" {result.stderr[:500]}") - continue - - # Find the JSON results (saved to home directory with timestamp) - import glob - - json_pattern = str(Path.home() / "or_scene_evaluation_results_*.json") - json_files = sorted( - glob.glob(json_pattern), key=lambda x: Path(x).stat().st_mtime, reverse=True - ) - - if len(json_files) > 0: - latest_json = json_files[0] - with open(latest_json, "r") as f: - all_results[prefix] = json.load(f) - print(f" ✓ Evaluation complete for {prefix}") - print(f" JSON: {latest_json}") - else: - print(f" WARNING: JSON output not found for {prefix}") - - # Step 3: Merge all metric bags into output bag - print(f"\nMerging metrics into output bag: {output_bag_path}") - - # Collect all topics and messages from original bag - reader = SequentialReader() - reader.open(StorageOptions(uri=bag_path, storage_id=""), ConverterOptions("cdr", "cdr")) - - all_topics = {} - for meta in reader.get_all_topics_and_types(): - all_topics[meta.name] = meta - - all_messages = [] - while reader.has_next(): - topic, data, timestamp = reader.read_next() - all_messages.append((topic, data, timestamp)) - - del reader - - # Collect metric topics and messages from each model - for prefix in all_results.keys(): - metric_bag = f"/tmp/eval_{prefix}_metrics.bag" - - if not Path(metric_bag).exists(): - continue - - print(f" Reading metrics for {prefix}...") - - metric_reader = SequentialReader() - metric_reader.open( - StorageOptions(uri=metric_bag, storage_id=""), ConverterOptions("cdr", "cdr") - ) - - # Collect metric topics (only those with messages) - metric_topics = {} - for meta in metric_reader.get_all_topics_and_types(): - metric_topics[meta.name] = meta - - # Collect metric messages - topics_with_messages = set() - while metric_reader.has_next(): - topic, data, timestamp = metric_reader.read_next() - all_messages.append((topic, data, timestamp)) - topics_with_messages.add(topic) - - del metric_reader - - # Add metric topics that have messages - for topic_name, meta in metric_topics.items(): - if topic_name in topics_with_messages: - all_topics[topic_name] = meta - - print(f" ✓ Added metrics for {prefix}") - - print(f" Total messages: {len(all_messages)}") - print(f" Total topics: {len(all_topics)}") - - # Write to temp bag - import shutil - import tempfile - - temp_bag = tempfile.mkdtemp(prefix="eval_merge_") - shutil.rmtree(temp_bag) # Remove the directory, we just need the unique name - - writer = SequentialWriter() - writer.open(StorageOptions(uri=temp_bag, storage_id="mcap"), ConverterOptions("cdr", "cdr")) - - # Create all topics - for meta in all_topics.values(): - writer.create_topic(meta) - - # Sort messages by timestamp (CRITICAL!) - all_messages.sort(key=lambda x: x[2]) - - # Write all messages - for topic, data, timestamp in all_messages: - writer.write(topic, data, timestamp) - - del writer - - # Skip Python reindexing - use CLI instead to preserve message schemas - # print(" Reindexing...") - # Reindexer().reindex(StorageOptions(uri=temp_bag, storage_id="mcap")) - - # Move to final location - if Path(output_bag_path).exists(): - shutil.rmtree(output_bag_path) - shutil.move(temp_bag, output_bag_path) - - # Save combined JSON - if json_output_path: - with open(json_output_path, "w") as f: - json.dump(all_results, f, indent=2) - print(f" ✓ Combined results saved to: {json_output_path}") - - print(f"\n✓ Evaluation complete for {len(all_results)} models") - print(f" Output bag: {output_bag_path}") - - -def main(): - parser = argparse.ArgumentParser( - description="Evaluate all LIVE trajectories in a bag simultaneously" - ) - parser.add_argument("--bag", required=True, help="Bag with LIVE trajectories") - parser.add_argument("--input-bag", required=True, help="Original input bag for OR extraction") - parser.add_argument("--map-path", required=True, help="Path to lanelet2 map file") - parser.add_argument("--output-bag", required=True, help="Output bag with metrics") - parser.add_argument("--json-output", help="Combined JSON output file") - parser.add_argument( - "--time-window", - type=float, - default=5.0, - help="Evaluation window on each side of OR (default: 5.0s)", - ) - - args = parser.parse_args() - - if not Path(args.bag).exists(): - print(f"ERROR: Bag not found: {args.bag}") - sys.exit(1) - - if not Path(args.input_bag).exists(): - print(f"ERROR: Input bag not found: {args.input_bag}") - sys.exit(1) - - if not Path(args.map_path).exists(): - print(f"ERROR: Map not found: {args.map_path}") - sys.exit(1) - - rclpy.init() - - evaluate_all_trajectories( - args.bag, args.input_bag, args.map_path, args.output_bag, args.json_output, args.time_window - ) - - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/evaluate_or_segments.py b/planning/autoware_planning_data_analyzer/scripts/evaluate_or_segments.py deleted file mode 100755 index 5231fa9a1..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/evaluate_or_segments.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable -"""Auto-discover and evaluate all OR segment bags in a directory.""" - -import argparse -from pathlib import Path -import subprocess -import sys - - -def find_or_segment_bags(input_dir: str): - """Find all or_event_* bags in input directory.""" - bags = [] - input_path = Path(input_dir) - - # Look for or_event_N directories (N = 0, 1, 2, ...) - for item in sorted(input_path.iterdir()): - if item.is_dir() and item.name.startswith("or_event_") and item.name[9:].isdigit(): - # Check if it has MCAP files - mcap_files = list(item.glob("*.mcap")) - if len(mcap_files) > 0: - segment_id = int(item.name[9:]) - bags.append({"segment_id": segment_id, "path": str(item), "name": item.name}) - - return sorted(bags, key=lambda x: x["segment_id"]) - - -def evaluate_all_segments(input_dir: str, map_path: str, time_window: float = 5.0): - """Evaluate all OR segment bags found in input directory.""" - print(f"Searching for OR segment bags in: {input_dir}") - - segment_bags = find_or_segment_bags(input_dir) - - if len(segment_bags) == 0: - print("ERROR: No OR segment bags found!") - print(" Expected: or_event_0/, or_event_1/, ...") - sys.exit(1) - - print(f"Found {len(segment_bags)} OR segment bags:") - for bag in segment_bags: - print(f" {bag['name']}") - - print("") - - # Evaluate each segment - for bag in segment_bags: - segment_id = bag["segment_id"] - bag_path = bag["path"] - - output_bag = f"{input_dir}/or_event_{segment_id}_WITH_METRICS" - json_output = f"{input_dir}/or_event_{segment_id}_results.json" - - print(f"=== Evaluating Segment {segment_id} ===") - - cmd = [ - "python3", - str(Path(__file__).parent / "evaluate_all_live_trajectories.py"), - "--bag", - bag_path, - "--input-bag", - bag_path, # Use segment bag for OR extraction (has route already) - "--map-path", - map_path, - "--output-bag", - output_bag, - "--json-output", - json_output, - "--time-window", - str(time_window), - ] - - result = subprocess.run(cmd) - - if result.returncode != 0: - print(f"ERROR: Evaluation failed for segment {segment_id}") - sys.exit(1) - - print(f"✓ Segment {segment_id} complete") - print(f" Output: {output_bag}") - print(f" JSON: {json_output}") - print("") - - print(f"✓ All {len(segment_bags)} segments evaluated successfully") - - -def main(): - parser = argparse.ArgumentParser(description="Auto-discover and evaluate all OR segment bags") - parser.add_argument("--input-dir", required=True, help="Directory containing or_event_* bags") - parser.add_argument("--map-path", required=True, help="Path to lanelet2 map file") - parser.add_argument( - "--time-window", - type=float, - default=5.0, - help="Evaluation window on each side of OR in seconds (default: 5.0)", - ) - - args = parser.parse_args() - - if not Path(args.input_dir).exists(): - print(f"ERROR: Input directory not found: {args.input_dir}") - sys.exit(1) - - if not Path(args.map_path).exists(): - print(f"ERROR: Map not found: {args.map_path}") - sys.exit(1) - - evaluate_all_segments(args.input_dir, args.map_path, args.time_window) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/generate_or_visualization.py b/planning/autoware_planning_data_analyzer/scripts/generate_or_visualization.py deleted file mode 100755 index 2ddfc0674..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/generate_or_visualization.py +++ /dev/null @@ -1,341 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable - -"""Generate debug visualization for OR scene trajectory comparison.""" - -import json -import os -import sys - -import matplotlib - -matplotlib.use("Agg") # Non-interactive backend -import matplotlib.patches as patches # noqa: E402 -import matplotlib.pyplot as plt # noqa: E402 -import numpy as np # noqa: E402 - -# Import lanelet2 for map visualization -try: - from lanelet2.io import Origin # noqa: E402 - from lanelet2.io import load # noqa: E402 - - try: - from autoware_lanelet2_extension_python.projection import MGRSProjector - except ImportError: - try: - from lanelet2.projection import MGRSProjector - except ImportError: - from lanelet2.projection import UtmProjector as MGRSProjector - LANELET2_AVAILABLE = True -except ImportError: - LANELET2_AVAILABLE = False - print("Warning: lanelet2 not available, map visualization disabled") - - -def load_lanelet_map(map_path): - """Load lanelet2 map from OSM file.""" - if not LANELET2_AVAILABLE: - return None - try: - projector = MGRSProjector(Origin(0.0, 0.0)) - return load(map_path, projector) - except Exception as e: - print(f"Warning: Failed to load map from {map_path}: {e}") - return None - - -def get_lanelets_in_area(lanelet_map, x_min, x_max, y_min, y_max): - """Filter lanelets within visualization bounds.""" - visible_lanelets = [] - for lanelet in lanelet_map.laneletLayer: - for pt in lanelet.centerline: - if x_min <= pt.x <= x_max and y_min <= pt.y <= y_max: - visible_lanelets.append(lanelet) - break - return visible_lanelets - - -def plot_lanelet_boundaries(ax, lanelets): - """Plot lanelet boundaries as background layer.""" - for lanelet in lanelets: - left_x = [p.x for p in lanelet.leftBound] - left_y = [p.y for p in lanelet.leftBound] - right_x = [p.x for p in lanelet.rightBound] - right_y = [p.y for p in lanelet.rightBound] - - ax.plot( - left_x, left_y, color="lightgray", linewidth=1.0, alpha=0.6, linestyle="-", zorder=1 - ) - ax.plot( - right_x, right_y, color="lightgray", linewidth=1.0, alpha=0.6, linestyle="-", zorder=1 - ) - - center_x = [p.x for p in lanelet.centerline] - center_y = [p.y for p in lanelet.centerline] - ax.plot( - center_x, - center_y, - color="lightgray", - linewidth=0.5, - alpha=0.4, - linestyle="--", - zorder=1, - ) - - -def quat_to_yaw(qx, qy, qz, qw): - """Convert quaternion to yaw angle.""" - siny_cosp = 2.0 * (qw * qz + qx * qy) - cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) - return np.arctan2(siny_cosp, cosy_cosp) - - -def create_bbox_polygon(x, y, yaw, length, width): - """Create bounding box polygon corners in global frame.""" - # Half dimensions - half_l = length / 2.0 - half_w = width / 2.0 - - # Corners in vehicle frame (front-left, front-right, rear-right, rear-left) - corners_local = np.array( - [ - [half_l, half_w], # Front-left - [half_l, -half_w], # Front-right - [-half_l, -half_w], # Rear-right - [-half_l, half_w], # Rear-left - ] - ) - - # Rotation matrix - cos_yaw = np.cos(yaw) - sin_yaw = np.sin(yaw) - rot = np.array([[cos_yaw, -sin_yaw], [sin_yaw, cos_yaw]]) - - # Transform to global frame - corners_global = corners_local @ rot.T - corners_global[:, 0] += x - corners_global[:, 1] += y - - return corners_global - - -def generate_or_visualization(data_json_path, output_image_path): - """Generate bird's eye view plot of predicted vs GT trajectories.""" - with open(data_json_path, "r") as f: - data = json.load(f) - - # Extract data - pred_poses = data["predicted_trajectory"] - gt_poses = data["ground_truth_trajectory"] - metrics = data["metrics"] - event_info = data["event_info"] - objects = data.get("objects", []) - - # Create figure - fig, ax = plt.subplots(figsize=(14, 12)) - - # Calculate bounds first (needed for map filtering) - pred_x = [p["x"] for p in pred_poses] - pred_y = [p["y"] for p in pred_poses] - gt_x = [p["x"] for p in gt_poses] - gt_y = [p["y"] for p in gt_poses] - - all_traj_x = pred_x + gt_x - all_traj_y = pred_y + gt_y - - if not all_traj_x or not all_traj_y: - print("ERROR: No trajectory data to plot!") - return - - x_min = min(all_traj_x) - x_max = max(all_traj_x) - y_min = min(all_traj_y) - y_max = max(all_traj_y) - - x_range = x_max - x_min - y_range = y_max - y_min - x_epsilon = max(1.0, x_range * 0.1) - y_epsilon = max(1.0, y_range * 0.1) - - # Load and plot map (background layer, zorder=1) - map_path = data.get("map_path", None) - if map_path and os.path.exists(map_path) and LANELET2_AVAILABLE: - try: - lanelet_map = load_lanelet_map(map_path) - if lanelet_map: - visible_lanelets = get_lanelets_in_area( - lanelet_map, - x_min - x_epsilon, - x_max + x_epsilon, - y_min - y_epsilon, - y_max + y_epsilon, - ) - plot_lanelet_boundaries(ax, visible_lanelets) - print(f"Plotted {len(visible_lanelets)} lanelets from map") - except Exception as e: - print(f"Warning: Failed to plot map: {e}") - - # Plot trajectories (already extracted above for bounds calculation) - # Debug output (optional) - # print(f"Predicted trajectory: {len(pred_x)} points") - # print(f"GT trajectory: {len(gt_x)} points") - # print(f"Objects: {len(objects)}") - - # Make trajectories visible with thinner lines - ax.plot(gt_x, gt_y, "g-", linewidth=2.5, label="Ground Truth", alpha=0.9, zorder=5) - ax.plot( - pred_x, pred_y, "b--", linewidth=2, label="Predicted", alpha=0.9, zorder=5, dashes=(5, 2) - ) - - # Mark start and end points (same size as trajectory line markers) - ax.plot(gt_x[0], gt_y[0], "go", markersize=8, label="GT Start", zorder=6) - ax.plot(gt_x[-1], gt_y[-1], "gs", markersize=8, label="GT End", zorder=6) - ax.plot(pred_x[0], pred_y[0], "bo", markersize=8, label="Pred Start", zorder=6) - ax.plot(pred_x[-1], pred_y[-1], "bs", markersize=8, label="Pred End", zorder=6) - - # Mark OR event location (vehicle position at OR) - or_x = event_info["vehicle_x_at_or"] - or_y = event_info["vehicle_y_at_or"] - ax.plot(or_x, or_y, "r*", markersize=15, label="OR Event", zorder=10, markeredgewidth=1.5) - - # Plot objects as bounding boxes - class_colors = { - 1: "cyan", # CAR - 2: "orange", # TRUCK - 3: "purple", # BUS - 6: "yellow", # BICYCLE - 7: "pink", # PEDESTRIAN - } - - class_names = { - 1: "Car", - 2: "Truck", - 3: "Bus", - 6: "Bicycle", - 7: "Pedestrian", - } - - # Track which object types are present for legend - present_classes = set() - - for i, obj in enumerate(objects): - # Get object pose - obj_x = obj["x"] - obj_y = obj["y"] - - # Calculate yaw from quaternion - quat = obj["orientation"] - yaw = quat_to_yaw(quat["x"], quat["y"], quat["z"], quat["w"]) - - # Use footprint if available, otherwise create bbox from dimensions - if "footprint" in obj and obj["footprint"]: - # Use provided footprint polygon - footprint_pts = np.array([[pt["x"], pt["y"]] for pt in obj["footprint"]]) - polygon = patches.Polygon( - footprint_pts, fill=False, edgecolor="gray", linewidth=1.5, linestyle="-", alpha=0.7 - ) - else: - # Create bounding box from dimensions - length = obj["length"] - width = obj["width"] - corners = create_bbox_polygon(obj_x, obj_y, yaw, length, width) - - # Get color based on class - class_label = obj.get("class_label", 0) - color = class_colors.get(class_label, "gray") - present_classes.add(class_label) - - polygon = patches.Polygon( - corners, fill=False, edgecolor=color, linewidth=2.5, linestyle="-", alpha=0.8 - ) - - ax.add_patch(polygon) - - # Add metrics text box - coverage_pct = metrics.get("gt_coverage_ratio", 1.0) * 100 - num_objects = len(objects) - - # Calculate distance between starts - if pred_x and gt_x: - start_dist = np.sqrt((pred_x[0] - gt_x[0]) ** 2 + (pred_y[0] - gt_y[0]) ** 2) - else: - start_dist = 0.0 - - metrics_text = f"""OR Scene Evaluation -Event #{event_info['event_id']} -Prediction: t={metrics['time_relative_to_or']:+.3f}s -Vehicle Speed: {event_info['vehicle_speed_at_or']:.2f} m/s - -ADE: {metrics['ade']:.3f} m -FDE: {metrics['fde']:.3f} m -Lateral Dev: {metrics['mean_lateral_deviation']:+.3f} m -Start Dist: {start_dist:.3f} m -Min TTC: {metrics['min_ttc']:.1f} s - -Coverage: {coverage_pct:.1f}% -Points: {metrics['num_points']}/{metrics.get('num_points_original', metrics['num_points'])} -Duration: {metrics['trajectory_duration']:.1f}/{metrics.get('trajectory_duration_original', metrics['trajectory_duration']):.1f} s -Objects: {num_objects}""" - - ax.text( - 0.02, - 0.98, - metrics_text, - transform=ax.transAxes, - fontsize=10, - verticalalignment="top", - bbox={"boxstyle": "round", "facecolor": "wheat", "alpha": 0.8}, - family="monospace", - ) - - # Set bounds with epsilon (already calculated above for map filtering) - ax.set_xlim(x_min - x_epsilon, x_max + x_epsilon) - ax.set_ylim(y_min - y_epsilon, y_max + y_epsilon) - - # Add object type legend entries (only for classes present in scene) - for class_id in sorted(present_classes): - if class_id in class_colors and class_id in class_names: - ax.plot( - [], - [], - color=class_colors[class_id], - linewidth=2.5, - label=class_names[class_id], - alpha=0.8, - ) - - # Formatting - ax.set_xlabel("X (meters)", fontsize=12) - ax.set_ylabel("Y (meters)", fontsize=12) - ax.set_title(f'OR Scene Trajectory Comparison\n{event_info["bag_name"]}', fontsize=14) - ax.legend(loc="upper right", fontsize=9, framealpha=0.9) - ax.grid(True, alpha=0.3) - - # Save - plt.tight_layout() - plt.savefig(output_image_path, dpi=150, bbox_inches="tight") - plt.close() - - print(f"Saved visualization to: {output_image_path}") - - -if __name__ == "__main__": - if len(sys.argv) != 3: - print("Usage: generate_or_visualization.py ") - sys.exit(1) - - generate_or_visualization(sys.argv[1], sys.argv[2]) diff --git a/planning/autoware_planning_data_analyzer/scripts/merge_bags.py b/planning/autoware_planning_data_analyzer/scripts/merge_bags.py deleted file mode 100755 index 6c1611a6d..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/merge_bags.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable -"""Merge multiple rosbag2 bags into a single output bag.""" - -import argparse -from pathlib import Path -import sys - -from rosbag2_py import ConverterOptions -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions - - -def merge_bags(input_bags: list[str], output_bag: str, storage_id: str = "mcap") -> None: - """ - Merge multiple input bags into a single output bag. - - Args: - input_bags: List of input bag paths - output_bag: Output bag path - storage_id: Storage format (default: mcap) - """ - print(f"Merging {len(input_bags)} bags into {output_bag}") - print(f"Input bags: {input_bags}") - - # Set up writer - writer = SequentialWriter() - storage_options = StorageOptions(uri=output_bag, storage_id=storage_id) - converter_options = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - - writer.open(storage_options, converter_options) - - # Track topics we've already added to avoid duplicates - added_topics = set() - - # Process each input bag - for bag_path in input_bags: - print(f"\nProcessing: {bag_path}") - - reader = SequentialReader() - input_storage = StorageOptions(uri=bag_path, storage_id="") - input_converter = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - - try: - reader.open(input_storage, input_converter) - except Exception as e: - print(f"ERROR: Failed to open {bag_path}: {e}") - continue - - # Get metadata - metadata = reader.get_metadata() - print(f" Topics: {len(metadata.topics_with_message_count)}") - print(f" Messages: {metadata.message_count}") - print(f" Duration: {metadata.duration.nanoseconds / 1e9:.2f}s") - - # Add topics from this bag - topic_types = reader.get_all_topics_and_types() - for topic_metadata in topic_types: - topic_name = topic_metadata.name - if topic_name not in added_topics: - writer.create_topic(topic_metadata) - added_topics.add(topic_name) - print(f" Added topic: {topic_name}") - - # Copy all messages - message_count = 0 - while reader.has_next(): - topic, data, timestamp = reader.read_next() - writer.write(topic, data, timestamp) - message_count += 1 - - if message_count % 1000 == 0: - print(f" Copied {message_count} messages...", end="\r") - - print(f" Copied {message_count} messages total") - - # Close writer - print(f"\nMerge complete! Output: {output_bag}") - print(f"Total topics: {len(added_topics)}") - - -def main(): - parser = argparse.ArgumentParser( - description="Merge multiple rosbag2 bags into one", - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=""" -Examples: - # Merge DLR result + evaluation metrics - python3 merge_bags.py \\ - --input /tmp/result_bag/result_bag_0.mcap /tmp/eval_metrics.bag \\ - --output /tmp/final_combined.bag \\ - --storage mcap - - # Merge multiple evaluation runs - python3 merge_bags.py \\ - --input run1.bag run2.bag run3.bag \\ - --output all_runs_combined.bag - """, - ) - - parser.add_argument( - "--input", "-i", nargs="+", required=True, help="Input bag paths (space-separated)" - ) - parser.add_argument("--output", "-o", required=True, help="Output bag path") - parser.add_argument( - "--storage", - "-s", - default="mcap", - choices=["mcap", "sqlite3"], - help="Output storage format (default: mcap)", - ) - - args = parser.parse_args() - - # Validate inputs exist - for bag_path in args.input: - if not Path(bag_path).exists(): - print(f"ERROR: Input bag does not exist: {bag_path}") - sys.exit(1) - - # Check output doesn't exist - if Path(args.output).exists(): - print(f"ERROR: Output bag already exists: {args.output}") - print("Please remove it first or choose a different output path") - sys.exit(1) - - try: - merge_bags(args.input, args.output, args.storage) - except Exception as e: - print(f"ERROR: Merge failed: {e}") - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/merge_segment_models.py b/planning/autoware_planning_data_analyzer/scripts/merge_segment_models.py deleted file mode 100755 index 9f19da08f..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/merge_segment_models.py +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable -"""Merge multiple model result bags for a single OR segment.""" - -import argparse -from pathlib import Path -import shutil -import sys -import tempfile - -from rosbag2_py import ConverterOptions -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions - - -def merge_model_bags(output_dir: str, segment_id: int, model_names: list, final_output: str): - """Merge all model result bags for one segment.""" - print(f"Merging segment {segment_id} models: {', '.join(model_names)}") - - temp_bag = tempfile.mkdtemp(prefix="merge_segment_") - shutil.rmtree(temp_bag) - - try: - writer = SequentialWriter() - writer.open(StorageOptions(uri=temp_bag, storage_id="mcap"), ConverterOptions("cdr", "cdr")) - - all_topics = {} - all_messages = [] - - # Read each model's result bag - for model_name in model_names: - result_bag = f"{output_dir}/or_event_{segment_id}_{model_name}_output/result_bag" - - if not Path(result_bag).exists(): - print(f" WARNING: Result bag not found: {result_bag}") - continue - - print(f" Reading {model_name} results...") - - reader = SequentialReader() - reader.open( - StorageOptions(uri=result_bag, storage_id=""), ConverterOptions("cdr", "cdr") - ) - - # Collect topics - for meta in reader.get_all_topics_and_types(): - if meta.name not in all_topics: - all_topics[meta.name] = meta - - # Collect messages (skip unprefixed trajectory) - while reader.has_next(): - topic, data, timestamp = reader.read_next() - - # Skip unprefixed trajectory - we only want prefixed versions - if ( - topic - == "/planning/trajectory_generator/diffusion_planner_node/output/trajectory" - ): - continue - - all_messages.append((topic, data, timestamp)) - - del reader - - if len(all_messages) == 0: - print("ERROR: No messages to merge!") - sys.exit(1) - - print(f" Total messages collected: {len(all_messages)}") - print(f" Total topics: {len(all_topics)}") - - # Create all topics - for meta in all_topics.values(): - # Skip unprefixed trajectory topic - if ( - meta.name - == "/planning/trajectory_generator/diffusion_planner_node/output/trajectory" - ): - continue - writer.create_topic(meta) - - # Sort messages by timestamp - all_messages.sort(key=lambda x: x[2]) - - # Write all messages - for topic, data, timestamp in all_messages: - writer.write(topic, data, timestamp) - - del writer - - # Skip Python reindexing - preserves message schemas for Lichtblick - # print(" Reindexing...") - # Reindexer().reindex(StorageOptions(uri=temp_bag, storage_id="mcap")) - - # Move to final location - if Path(final_output).exists(): - shutil.rmtree(final_output) - shutil.move(temp_bag, final_output) - - print(f" ✓ Merged {len(model_names)} models into {final_output}") - - except Exception as e: - if Path(temp_bag).exists(): - shutil.rmtree(temp_bag) - raise e - - -def main(): - parser = argparse.ArgumentParser(description="Merge model result bags for OR segment") - parser.add_argument("--output-dir", required=True, help="Base output directory") - parser.add_argument("--segment-id", type=int, required=True, help="Segment ID") - parser.add_argument("--models", nargs="+", required=True, help="Model names (e.g., v2.0 v2.1)") - parser.add_argument("--final-output", required=True, help="Final merged bag directory") - - args = parser.parse_args() - - merge_model_bags(args.output_dir, args.segment_id, args.models, args.final_output) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/scripts/rename_bag_topic.py b/planning/autoware_planning_data_analyzer/scripts/rename_bag_topic.py deleted file mode 100755 index 31cd0b908..000000000 --- a/planning/autoware_planning_data_analyzer/scripts/rename_bag_topic.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. -# cspell:disable - -"""Rename a topic in a rosbag file.""" - -import argparse -from pathlib import Path - -from rosbag2_py import ConverterOptions -from rosbag2_py import Reindexer -from rosbag2_py import SequentialReader -from rosbag2_py import SequentialWriter -from rosbag2_py import StorageOptions -from rosbag2_py import TopicMetadata - - -def rename_topic_in_bag(input_bag, output_bag, old_topic, new_topic): - """ - Copy a bag while renaming a specific topic. - - Args: - input_bag: Path to input bag directory or mcap file - output_bag: Path to output bag directory - old_topic: Original topic name to rename - new_topic: New topic name - """ - print("Renaming topic in bag:") - print(f" Input: {input_bag}") - print(f" Output: {output_bag}") - print(" {old_topic} → {new_topic}") - print("") - - # Auto-detect storage type - input_path = Path(input_bag) - if input_path.is_file() and input_path.suffix == ".mcap": - storage_id = "mcap" - uri = str(input_path) - elif input_path.is_dir(): - storage_id = "sqlite3" - uri = str(input_path) - else: - raise ValueError(f"Invalid input bag path: {input_bag}") - - # Setup reader - storage_options_in = StorageOptions(uri=uri, storage_id=storage_id) - converter_options = ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - - reader = SequentialReader() - reader.open(storage_options_in, converter_options) - - # Setup writer (always output as mcap for consistency) - storage_options_out = StorageOptions(uri=str(output_bag), storage_id="mcap") - writer = SequentialWriter() - writer.open(storage_options_out, converter_options) - - # Create topics in output (with renaming) - topic_renamed = False - for topic_metadata in reader.get_all_topics_and_types(): - output_topic_name = topic_metadata.name - - if topic_metadata.name == old_topic: - output_topic_name = new_topic - topic_renamed = True - print(f"✓ Renamed topic: {old_topic} → {new_topic}") - - new_metadata = TopicMetadata( - name=output_topic_name, - type=topic_metadata.type, - serialization_format=topic_metadata.serialization_format, - ) - writer.create_topic(new_metadata) - - if not topic_renamed: - print(f"Warning: Topic {old_topic} not found in bag!") - - # Copy all messages (with topic renaming) - message_count = 0 - renamed_count = 0 - - while reader.has_next(): - topic, data, timestamp = reader.read_next() - - # Rename if needed - if topic == old_topic: - topic = new_topic - renamed_count += 1 - - writer.write(topic, data, timestamp) - message_count += 1 - - if message_count % 10000 == 0: - print(f"Processed {message_count} messages...") - - print("\nCopy complete:") - print(f" Total messages: {message_count}") - print(f" Renamed messages: {renamed_count}") - - # Cleanup - del writer - del reader - - # Reindex for clean metadata - print("Reindexing...") - Reindexer().reindex(storage_options_out) - print("Done!") - - -def main(): - parser = argparse.ArgumentParser(description="Rename a topic in a rosbag file") - parser.add_argument("input_bag", help="Input bag directory or mcap file") - parser.add_argument("output_bag", help="Output bag directory") - parser.add_argument("old_topic", help="Topic name to rename") - parser.add_argument("new_topic", help="New topic name") - - args = parser.parse_args() - - rename_topic_in_bag(args.input_bag, args.output_bag, args.old_topic, args.new_topic) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp b/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp index 073c055f3..8b8696991 100644 --- a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp +++ b/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.cpp @@ -17,7 +17,6 @@ // #include "closed_loop_evaluator.hpp" // TODO(gosakayori): Closed loop evaluator not yet // implemented #include "open_loop_evaluator.hpp" -#include "or_scene_evaluator.hpp" #include "utils/path_utils.hpp" #include @@ -178,11 +177,8 @@ AutowarePlanningDataAnalyzerNode::AutowarePlanningDataAnalyzerNode( const auto mode_str = get_or_declare_parameter(*this, "evaluation.mode"); if (mode_str == "open_loop") { evaluation_mode_ = EvaluationMode::OPEN_LOOP; - } else if (mode_str == "or_scene") { - evaluation_mode_ = EvaluationMode::OR_SCENE; } else { - RCLCPP_ERROR(get_logger(), "Invalid evaluation mode: %s. Using OPEN_LOOP.", mode_str.c_str()); - evaluation_mode_ = EvaluationMode::OPEN_LOOP; + throw std::runtime_error("Invalid evaluation mode: " + mode_str + ". Expected 'open_loop'."); } run_evaluation(); @@ -446,17 +442,12 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation() } } - // Route is optional for OR scene evaluation (doesn't use lane-based metrics) - if (!last_route_msg && evaluation_mode_ != EvaluationMode::OR_SCENE) { + if (!last_route_msg) { RCLCPP_WARN(get_logger(), "No route message found in bag. Evaluation aborted."); return; } - if (last_route_msg) { - route_handler_->setRoute(*last_route_msg); - } else { - RCLCPP_INFO(get_logger(), "No route found in bag (OK for OR scene evaluation)"); - } + route_handler_->setRoute(*last_route_msg); // Seek back to the beginning of the bag for mode-specific evaluation bag_reader_.seek(0); @@ -512,87 +503,6 @@ void AutowarePlanningDataAnalyzerNode::run_evaluation() end_time = times.second; break; } - case EvaluationMode::OR_SCENE: { - // Read OR scene specific parameters - const double time_window_sec = - get_or_declare_parameter(*this, "or_scene_evaluation.time_window_sec"); - const bool enable_debug_viz = - get_or_declare_parameter(*this, "or_scene_evaluation.enable_debug_visualization"); - - // Read success criteria - ORSuccessCriteria success_criteria; - success_criteria.enabled = - get_or_declare_parameter(*this, "or_scene_evaluation.success_criteria.enabled"); - success_criteria.max_ade = - get_or_declare_parameter(*this, "or_scene_evaluation.success_criteria.max_ade"); - success_criteria.max_fde = - get_or_declare_parameter(*this, "or_scene_evaluation.success_criteria.max_fde"); - success_criteria.max_lateral_deviation = get_or_declare_parameter( - *this, "or_scene_evaluation.success_criteria.max_lateral_deviation"); - success_criteria.min_ttc = - get_or_declare_parameter(*this, "or_scene_evaluation.success_criteria.min_ttc"); - - const auto debug_output_dir = - get_or_declare_parameter(*this, "or_scene_evaluation.debug_output_dir"); - - ORSceneEvaluator evaluator( - get_logger(), route_handler_, time_window_sec, success_criteria, enable_debug_viz, - debug_output_dir); - evaluator.set_json_output_dir(output_dir_path.string()); - - // Set OR events JSON paths if provided - const auto or_events_input_path = - get_or_declare_parameter(*this, "or_scene_evaluation.or_events_input_path"); - const auto or_events_output_path = - get_or_declare_parameter(*this, "or_scene_evaluation.or_events_output_path"); - const auto input_bag_path = - get_or_declare_parameter(*this, "or_scene_evaluation.input_bag_path"); - - if (!or_events_input_path.empty()) { - evaluator.set_or_events_json_path(or_events_input_path); - } - if (!or_events_output_path.empty()) { - evaluator.set_or_events_output_path(or_events_output_path); - } - if (!input_bag_path.empty()) { - evaluator.set_input_bag_path(input_bag_path); - } - - // Set map path for visualization - auto map_path = get_or_declare_parameter(*this, "or_scene_evaluation.map_path"); - if (!map_path.empty()) { - evaluator.set_map_path(map_path); - } - - // Extract metric topic prefix from trajectory topic for multi-run support - // Topics like /model_v1/planning/.../trajectory → prefix "model_v1" - // Standard topics like /planning/.../trajectory → no prefix - std::string trajectory_topic = topic_names.trajectory_topic; - if (!trajectory_topic.empty() && trajectory_topic[0] == '/') { - // Find second slash (end of first component) - size_t second_slash = trajectory_topic.find('/', 1); - if (second_slash != std::string::npos) { - std::string first_component = trajectory_topic.substr(1, second_slash - 1); - - // Check if first component is NOT a standard Autoware namespace - const std::set standard_namespaces = { - "planning", "control", "localization", "perception", - "sensing", "map", "system", "vehicle"}; - - if (standard_namespaces.find(first_component) == standard_namespaces.end()) { - // This is a custom prefix for multi-run collection - evaluator.set_metric_topic_prefix(first_component); - RCLCPP_INFO(get_logger(), "Using metric topic prefix: %s", first_component.c_str()); - } - } - } - - auto times = - evaluator.run_evaluation_from_bag(bag_path_, evaluation_bag_writer_.get(), topic_names); - start_time = times.first; - end_time = times.second; - break; - } } if (!bag_time_initialized) { diff --git a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp b/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp index aae776902..d218fd8aa 100644 --- a/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp +++ b/planning/autoware_planning_data_analyzer/src/autoware_planning_data_analyzer_node.hpp @@ -56,7 +56,7 @@ class AutowarePlanningDataAnalyzerNode : public rclcpp::Node ~AutowarePlanningDataAnalyzerNode() override; private: - enum class EvaluationMode { OPEN_LOOP, OR_SCENE }; + enum class EvaluationMode { OPEN_LOOP }; void setup_evaluation_bag_writer(); void close_evaluation_bag_writer(); diff --git a/planning/autoware_planning_data_analyzer/src/or_event_extractor.cpp b/planning/autoware_planning_data_analyzer/src/or_event_extractor.cpp deleted file mode 100644 index 786962fd4..000000000 --- a/planning/autoware_planning_data_analyzer/src/or_event_extractor.cpp +++ /dev/null @@ -1,269 +0,0 @@ -// Copyright 2025 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. - -#include "or_event_extractor.hpp" - -#include "utils/bag_utils.hpp" -#include "utils/json_utils.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::planning_data_analyzer -{ - -OREventExtractor::OREventExtractor(rclcpp::Logger logger, double time_window_sec) -: logger_(logger), time_window_sec_(time_window_sec) -{ - RCLCPP_INFO(logger_, "OREventExtractor initialized with time_window=%.2fs", time_window_sec_); -} - -std::vector OREventExtractor::extract_or_events_from_bag( - const std::string & bag_path, const std::string & control_mode_topic, - const std::string & kinematic_state_topic) -{ - RCLCPP_INFO(logger_, "Extracting OR events from bag: %s", bag_path.c_str()); - RCLCPP_INFO(logger_, " Control mode topic: %s", control_mode_topic.c_str()); - RCLCPP_INFO(logger_, " Kinematic state topic: %s", kinematic_state_topic.c_str()); - - std::vector or_events; - - // Open bag - rosbag2_cpp::Reader bag_reader; - if (!utils::open_bag(bag_reader, bag_path, logger_)) { - return or_events; - } - - // Filter topics - utils::set_topic_filter(bag_reader, {control_mode_topic, kinematic_state_topic}); - - // State tracking - uint8_t prev_mode = 255; // Invalid initial value - bool first_message = true; - - // Buffer for odometry messages (for speed at OR) - std::vector> odometry_buffer; - constexpr size_t MAX_ODOMETRY_BUFFER_SIZE = 1000; - - // Read bag - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - - // Process control mode messages - if (bag_message->topic_name == control_mode_topic) { - auto control_mode_msg = utils::deserialize_message(bag_message); - - const uint8_t current_mode = control_mode_msg->mode; - - // Check for OR transition (skip first message) - if (!first_message && is_or_transition(prev_mode, current_mode)) { - OREvent event; - event.timestamp = rclcpp::Time(control_mode_msg->stamp); - event.prev_mode = prev_mode; - event.new_mode = current_mode; - event.prev_mode_str = get_mode_name(prev_mode); - event.new_mode_str = get_mode_name(current_mode); - - // Calculate time window (symmetric around OR) - const rclcpp::Duration window_duration( - static_cast(time_window_sec_), - static_cast((time_window_sec_ - static_cast(time_window_sec_)) * 1e9)); - event.time_window = window_duration; - event.window_start = event.timestamp - window_duration; - event.window_end = event.timestamp + window_duration; - - // Find vehicle speed and position at OR time - auto closest_odom = find_closest_odometry(event.timestamp, odometry_buffer); - if (closest_odom) { - event.vehicle_speed_mps = closest_odom->twist.twist.linear.x; - event.vehicle_x_at_or = closest_odom->pose.pose.position.x; - event.vehicle_y_at_or = closest_odom->pose.pose.position.y; - } else { - event.vehicle_speed_mps = 0.0; - event.vehicle_x_at_or = 0.0; - event.vehicle_y_at_or = 0.0; - RCLCPP_WARN( - logger_, "Could not find odometry for OR event at time %.3f", - event.timestamp.seconds()); - } - - or_events.push_back(event); - - RCLCPP_INFO( - logger_, "OR Event #%zu detected at t=%.3fs: %s → %s (speed: %.2f m/s)", or_events.size(), - event.timestamp.seconds(), event.prev_mode_str.c_str(), event.new_mode_str.c_str(), - event.vehicle_speed_mps); - } - - prev_mode = current_mode; - first_message = false; - } - // Process odometry messages (buffer for speed lookup) - if (bag_message->topic_name == kinematic_state_topic) { - auto odometry_msg = utils::deserialize_message(bag_message); - odometry_buffer.push_back(odometry_msg); - - // Keep buffer size reasonable - if (odometry_buffer.size() > MAX_ODOMETRY_BUFFER_SIZE) { - odometry_buffer.erase(odometry_buffer.begin(), odometry_buffer.begin() + 100); - } - } - } - - RCLCPP_INFO(logger_, "Extraction complete: %zu OR events found", or_events.size()); - - return or_events; -} - -void OREventExtractor::save_or_events_to_json( - const std::vector & events, const std::string & output_path) -{ - nlohmann::json j; - j["time_window_sec"] = time_window_sec_; - j["total_or_events"] = events.size(); - - nlohmann::json events_array = nlohmann::json::array(); - for (size_t i = 0; i < events.size(); ++i) { - const auto & event = events[i]; - nlohmann::json event_json; - - event_json["event_id"] = i; - event_json["timestamp"] = event.timestamp.seconds(); - event_json["window_start"] = event.window_start.seconds(); - event_json["window_end"] = event.window_end.seconds(); - event_json["time_window_sec"] = event.time_window.seconds(); - - event_json["vehicle_speed_mps"] = event.vehicle_speed_mps; - event_json["vehicle_x_at_or"] = event.vehicle_x_at_or; - event_json["vehicle_y_at_or"] = event.vehicle_y_at_or; - event_json["prev_mode"] = event.prev_mode; - event_json["new_mode"] = event.new_mode; - event_json["prev_mode_str"] = event.prev_mode_str; - event_json["new_mode_str"] = event.new_mode_str; - - events_array.push_back(event_json); - } - j["or_events"] = events_array; - - // Write to file - if (utils::save_json_to_file(j, output_path, logger_)) { - RCLCPP_INFO(logger_, "Saved %zu OR events", events.size()); - } -} - -std::vector OREventExtractor::load_or_events_from_json(const std::string & json_path) -{ - std::vector events; - - auto j = utils::load_json_from_file(json_path, logger_); - if (j.empty() || !j.contains("or_events")) { - RCLCPP_ERROR(logger_, "Invalid JSON file or missing 'or_events' field"); - return events; - } - - for (const auto & event_json : j["or_events"]) { - OREvent event; - - const double timestamp_sec = event_json["timestamp"]; - const double window_start_sec = event_json["window_start"]; - const double window_end_sec = event_json["window_end"]; - const double time_window_sec = event_json["time_window_sec"]; - - event.timestamp = rclcpp::Time(static_cast(timestamp_sec * 1e9), RCL_ROS_TIME); - event.window_start = rclcpp::Time(static_cast(window_start_sec * 1e9), RCL_ROS_TIME); - event.window_end = rclcpp::Time(static_cast(window_end_sec * 1e9), RCL_ROS_TIME); - event.time_window = rclcpp::Duration( - static_cast(time_window_sec), - static_cast((time_window_sec - static_cast(time_window_sec)) * 1e9)); - - event.vehicle_speed_mps = event_json["vehicle_speed_mps"]; - event.vehicle_x_at_or = event_json.value("vehicle_x_at_or", 0.0); - event.vehicle_y_at_or = event_json.value("vehicle_y_at_or", 0.0); - event.prev_mode = event_json["prev_mode"]; - event.new_mode = event_json["new_mode"]; - event.prev_mode_str = event_json["prev_mode_str"]; - event.new_mode_str = event_json["new_mode_str"]; - - events.push_back(event); - } - - RCLCPP_INFO(logger_, "Loaded %zu OR events from: %s", events.size(), json_path.c_str()); - - return events; -} - -bool OREventExtractor::is_or_transition(uint8_t prev_mode, uint8_t current_mode) const -{ - // OR event: AUTONOMOUS (1) → MANUAL (4) - return (prev_mode == ControlModeReport::AUTONOMOUS) && - (current_mode == ControlModeReport::MANUAL); -} - -std::string OREventExtractor::get_mode_name(uint8_t mode) const -{ - switch (mode) { - case ControlModeReport::NO_COMMAND: - return "NO_COMMAND"; - case ControlModeReport::AUTONOMOUS: - return "AUTONOMOUS"; - case ControlModeReport::AUTONOMOUS_STEER_ONLY: - return "AUTONOMOUS_STEER_ONLY"; - case ControlModeReport::AUTONOMOUS_VELOCITY_ONLY: - return "AUTONOMOUS_VELOCITY_ONLY"; - case ControlModeReport::MANUAL: - return "MANUAL"; - case ControlModeReport::DISENGAGED: - return "DISENGAGED"; - case ControlModeReport::NOT_READY: - return "NOT_READY"; - default: - return "INVALID"; - } -} - -std::shared_ptr OREventExtractor::find_closest_odometry( - const rclcpp::Time & target_time, const std::vector> & odometry_buffer, - const rclcpp::Duration & tolerance) const -{ - if (odometry_buffer.empty()) { - return nullptr; - } - - std::shared_ptr closest = nullptr; - double min_time_diff = std::numeric_limits::max(); - - for (const auto & odom : odometry_buffer) { - const rclcpp::Time odom_time(odom->header.stamp); - const double time_diff = std::abs((target_time - odom_time).seconds()); - - if (time_diff < min_time_diff) { - min_time_diff = time_diff; - closest = odom; - } - } - - // Check if within tolerance - if (min_time_diff > tolerance.seconds()) { - return nullptr; - } - - return closest; -} - -} // namespace autoware::planning_data_analyzer diff --git a/planning/autoware_planning_data_analyzer/src/or_event_extractor.hpp b/planning/autoware_planning_data_analyzer/src/or_event_extractor.hpp deleted file mode 100644 index 45c29b458..000000000 --- a/planning/autoware_planning_data_analyzer/src/or_event_extractor.hpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright 2025 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. - -#ifndef OR_EVENT_EXTRACTOR_HPP_ -#define OR_EVENT_EXTRACTOR_HPP_ - -#include "data_types.hpp" -#include "or_scene_structs.hpp" - -#include -#include -#include - -#include - -#include -#include -#include - -namespace autoware::planning_data_analyzer -{ - -/** - * @brief Extracts Override (OR) events from rosbag data (Stage 1) - * - * This class performs the first stage of OR scene evaluation: - * - Opens rosbag and reads operation_mode_state topic - * - Detects transitions from AUTONOMOUS to LOCAL mode - * - Records OR event timestamps and vehicle state - * - Saves/loads OR events to/from JSON for reuse - * - * Usage: - * OREventExtractor extractor(logger, 0.5); // 0.5s window on each side - * auto events = extractor.extract_or_events_from_bag(bag_path); - * extractor.save_or_events_to_json(events, "or_events.json"); - */ -class OREventExtractor -{ -public: - using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; - - /** - * @brief Constructor - * @param logger ROS logger for output - * @param time_window_sec Time window on each side of OR event (default: 0.5s) - * Creates evaluation window: [OR - time_window, OR + time_window] - */ - explicit OREventExtractor(rclcpp::Logger logger, double time_window_sec = 0.5); - - /** - * @brief Extract OR events from rosbag - * @param bag_path Path to input rosbag file - * @param control_mode_topic Topic name for vehicle control mode - * @param kinematic_state_topic Topic name for vehicle odometry (to get speed at OR) - * @return Vector of detected OR events - */ - std::vector extract_or_events_from_bag( - const std::string & bag_path, - const std::string & control_mode_topic = "/vehicle/status/control_mode", - const std::string & kinematic_state_topic = "/localization/kinematic_state"); - - /** - * @brief Save OR events to JSON file - * @param events Vector of OR events - * @param output_path Path to output JSON file - */ - void save_or_events_to_json(const std::vector & events, const std::string & output_path); - - /** - * @brief Load OR events from JSON file - * @param json_path Path to JSON file - * @return Vector of OR events - */ - std::vector load_or_events_from_json(const std::string & json_path); - -private: - rclcpp::Logger logger_; - double time_window_sec_; - - /** - * @brief Check if mode transition is an OR event (AUTONOMOUS → LOCAL) - * @param prev_mode Previous operation mode - * @param current_mode Current operation mode - * @return true if this is an OR transition - */ - bool is_or_transition(uint8_t prev_mode, uint8_t current_mode) const; - - /** - * @brief Get human-readable mode name - * @param mode Operation mode value - * @return Mode name string (e.g., "AUTONOMOUS", "LOCAL") - */ - std::string get_mode_name(uint8_t mode) const; - - /** - * @brief Find closest odometry message to given timestamp - * @param target_time Timestamp to find odometry for - * @param odometry_buffer Buffer of odometry messages - * @return Closest odometry message, or nullptr if not found - */ - std::shared_ptr find_closest_odometry( - const rclcpp::Time & target_time, - const std::vector> & odometry_buffer, - const rclcpp::Duration & tolerance = rclcpp::Duration(1, 0)) const; -}; - -} // namespace autoware::planning_data_analyzer - -#endif // OR_EVENT_EXTRACTOR_HPP_ diff --git a/planning/autoware_planning_data_analyzer/src/or_scene_evaluator.cpp b/planning/autoware_planning_data_analyzer/src/or_scene_evaluator.cpp deleted file mode 100644 index b5b62fa92..000000000 --- a/planning/autoware_planning_data_analyzer/src/or_scene_evaluator.cpp +++ /dev/null @@ -1,1134 +0,0 @@ -// Copyright 2025 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. - -#include "or_scene_evaluator.hpp" - -#include "bag_handler.hpp" - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::planning_data_analyzer -{ - -using autoware::route_handler::RouteHandler; - -// Helper for statistics calculation -template -struct Statistics -{ - double mean = 0.0; - double std_dev = 0.0; - double min_val = 0.0; - double max_val = 0.0; -}; - -template -Statistics calculate_statistics(const Container & values) -{ - Statistics stats; - - if (values.empty()) { - return stats; - } - - // Calculate mean - stats.mean = std::accumulate(values.begin(), values.end(), 0.0) / values.size(); - - // Calculate min/max - stats.min_val = *std::min_element(values.begin(), values.end()); - stats.max_val = *std::max_element(values.begin(), values.end()); - - // Calculate standard deviation - double variance = 0.0; - for (const auto & val : values) { - variance += (val - stats.mean) * (val - stats.mean); - } - stats.std_dev = std::sqrt(variance / values.size()); - - return stats; -} - -ORSceneEvaluator::ORSceneEvaluator( - rclcpp::Logger logger, std::shared_ptr route_handler, double time_window_sec, - const ORSuccessCriteria & success_criteria, bool enable_debug_visualization, - const std::string & debug_output_dir) -: BaseEvaluator(logger, route_handler), - time_window_sec_(time_window_sec), - success_criteria_(success_criteria), - enable_debug_visualization_(enable_debug_visualization), - debug_output_dir_(debug_output_dir) -{ - RCLCPP_INFO( - logger_, "ORSceneEvaluator initialized: time_window=%.2fs, debug_viz=%s", time_window_sec_, - enable_debug_visualization_ ? "enabled" : "disabled"); - - if (enable_debug_visualization_) { - RCLCPP_INFO(logger_, " Debug output directory: %s", debug_output_dir_.c_str()); - - // Create debug directory if it doesn't exist - std::filesystem::path debug_path(debug_output_dir_); - if (debug_path.string().substr(0, 2) == "~/") { - const char * home = std::getenv("HOME"); - if (home) { - debug_path = std::string(home) + debug_path.string().substr(1); - } - } - - if (!std::filesystem::exists(debug_path)) { - std::filesystem::create_directories(debug_path); - RCLCPP_INFO(logger_, " Created debug directory: %s", debug_path.c_str()); - } - } - - if (success_criteria_.enabled) { - RCLCPP_INFO(logger_, "Success criteria enabled:"); - RCLCPP_INFO(logger_, " max_ade=%.2fm", success_criteria_.max_ade); - RCLCPP_INFO(logger_, " max_fde=%.2fm", success_criteria_.max_fde); - RCLCPP_INFO(logger_, " max_lateral_deviation=%.2fm", success_criteria_.max_lateral_deviation); - RCLCPP_INFO(logger_, " min_ttc=%.2fs", success_criteria_.min_ttc); - } - - // Create OpenLoopEvaluator for reusing GT generation and comparison logic - open_loop_evaluator_ = std::make_unique(logger_, route_handler_); -} - -void ORSceneEvaluator::set_or_events_json_path(const std::string & json_path) -{ - or_events_json_input_path_ = json_path; - RCLCPP_INFO(logger_, "Will load OR events from: %s", json_path.c_str()); -} - -void ORSceneEvaluator::set_or_events_output_path(const std::string & json_path) -{ - or_events_json_output_path_ = json_path; - RCLCPP_INFO(logger_, "Will save OR events to: %s", json_path.c_str()); -} - -void ORSceneEvaluator::set_input_bag_path(const std::string & bag_path) -{ - input_bag_path_ = bag_path; - RCLCPP_INFO(logger_, "Will extract OR events from input bag: %s", bag_path.c_str()); -} - -void ORSceneEvaluator::set_map_path(const std::string & map_path) -{ - map_path_ = map_path; -} - -void ORSceneEvaluator::set_metric_topic_prefix(const std::string & prefix) -{ - metric_topic_prefix_ = prefix; -} - -std::pair ORSceneEvaluator::run_evaluation_from_bag( - const std::string & bag_path, rosbag2_cpp::Writer * evaluation_bag_writer, - const TopicNames & topic_names) -{ - RCLCPP_INFO(logger_, "Running OR scene evaluation"); - RCLCPP_INFO(logger_, " Bag path: %s", bag_path.c_str()); - RCLCPP_INFO( - logger_, " Time window: [OR - %.2fs, OR + %.2fs]", time_window_sec_, time_window_sec_); - - rclcpp::Time evaluation_start_time(0, 0, RCL_ROS_TIME); - rclcpp::Time evaluation_end_time(0, 0, RCL_ROS_TIME); - - // Stage 1: Extract OR events - or_events_ = extract_or_events(bag_path, topic_names); - - if (or_events_.empty()) { - RCLCPP_WARN(logger_, "No OR events found in bag"); - return {evaluation_start_time, evaluation_end_time}; - } - - RCLCPP_INFO(logger_, "Found %zu OR events, beginning evaluation...", or_events_.size()); - - // Initialize bag writer topics if provided - if (evaluation_bag_writer) { - create_topics_in_bag(*evaluation_bag_writer); - } - - // Stage 2: Evaluate each OR event - event_metrics_list_.clear(); - for (size_t i = 0; i < or_events_.size(); ++i) { - const auto & event = or_events_[i]; - RCLCPP_INFO( - logger_, "Evaluating OR event %zu/%zu at t=%.3fs", i + 1, or_events_.size(), - event.timestamp.seconds()); - - auto event_metrics = evaluate_or_event(event, bag_path, evaluation_bag_writer, topic_names); - event_metrics_list_.push_back(event_metrics); - - if (event_metrics_list_.size() == 1) { - evaluation_start_time = event.window_start; - } - evaluation_end_time = event.window_end; - } - - // Calculate summary - calculate_summary(); - - // Save results - auto summary_json = get_summary_as_json(); - auto detailed_json = get_detailed_results_as_json(); - save_json_results( - summary_json, bag_path, "or_scene", "time_step_based_trajectory_metric.json", false); - save_json_results( - detailed_json, bag_path, "or_scene", "time_step_based_trajectory_result.jsonl", false); - - // Log summary - RCLCPP_INFO(logger_, "OR scene evaluation complete:"); - RCLCPP_INFO(logger_, " Total OR events: %zu", summary_.total_or_events); - RCLCPP_INFO(logger_, " Events with predictions: %zu", summary_.events_with_valid_predictions); - RCLCPP_INFO( - logger_, " Mean ADE: %.3fm (±%.3fm)", summary_.mean_ade_all_events, - summary_.std_ade_all_events); - RCLCPP_INFO( - logger_, " Mean FDE: %.3fm (±%.3fm)", summary_.mean_fde_all_events, - summary_.std_fde_all_events); - - if (success_criteria_.enabled) { - RCLCPP_INFO( - logger_, " Events meeting criteria: %zu/%zu (%.1f%%)", summary_.events_meeting_criteria, - summary_.total_or_events, summary_.criteria_success_rate * 100.0); - } - - return {evaluation_start_time, evaluation_end_time}; -} - -void ORSceneEvaluator::evaluate( - const std::vector> & /* synchronized_data_list */, - rosbag2_cpp::Writer * /* bag_writer */) -{ - // This method is called by BaseEvaluator but not used in OR scene evaluation - // OR scene evaluation uses run_evaluation_from_bag directly - RCLCPP_WARN(logger_, "ORSceneEvaluator::evaluate() called but not implemented"); - RCLCPP_WARN(logger_, "Use run_evaluation_from_bag() instead"); -} - -std::vector ORSceneEvaluator::extract_or_events( - const std::string & bag_path, const TopicNames & topic_names) -{ - // Check if we should load from existing JSON - if (!or_events_json_input_path_.empty()) { - if (std::filesystem::exists(or_events_json_input_path_)) { - RCLCPP_INFO(logger_, "Loading pre-extracted OR events from JSON..."); - OREventExtractor extractor(logger_, time_window_sec_); - return extractor.load_or_events_from_json(or_events_json_input_path_); - } else { - RCLCPP_WARN( - logger_, "Specified OR events JSON not found: %s", or_events_json_input_path_.c_str()); - RCLCPP_INFO(logger_, "Falling back to extraction from bag"); - } - } - - // Extract OR events from bag - RCLCPP_INFO(logger_, "Extracting OR events from bag (Stage 1)..."); - OREventExtractor extractor(logger_, time_window_sec_); - - // Use default topic name for control mode - std::string control_mode_topic = "/vehicle/status/control_mode"; - - // Use input_bag_path if specified (for LIVE trajectory evaluation), otherwise use main bag_path - std::string bag_for_or_extraction = input_bag_path_.empty() ? bag_path : input_bag_path_; - RCLCPP_INFO(logger_, "Extracting OR events from: %s", bag_for_or_extraction.c_str()); - - auto events = extractor.extract_or_events_from_bag( - bag_for_or_extraction, control_mode_topic, topic_names.odometry_topic); - - // Save to JSON if output path specified - if (!or_events_json_output_path_.empty()) { - extractor.save_or_events_to_json(events, or_events_json_output_path_); - } - - return events; -} - -OREventMetrics ORSceneEvaluator::evaluate_or_event( - const OREvent & event, const std::string & bag_path, rosbag2_cpp::Writer * bag_writer, - const TopicNames & topic_names) -{ - OREventMetrics event_metrics; - event_metrics.event_id = event_metrics_list_.size(); - event_metrics.or_timestamp = event.timestamp; - event_metrics.window_start = event.window_start; - event_metrics.window_end = event.window_end; - event_metrics.vehicle_speed_at_or = event.vehicle_speed_mps; - - // Load ALL bag data (needed for GT interpolation over full trajectory horizon) - RCLCPP_INFO(logger_, "Loading entire bag for GT generation..."); - auto bag_result = process_bag_common(bag_path, nullptr, topic_names); - - if (bag_result.synchronized_data_list.empty()) { - RCLCPP_WARN(logger_, "OR event %zu: No synchronized data in bag", event_metrics.event_id); - return event_metrics; - } - - RCLCPP_INFO( - logger_, "Loaded %zu synchronized data points from bag", - bag_result.synchronized_data_list.size()); - - // Filter to get trajectory predictions in OR window only - std::vector> trajectories_in_window; - const auto window_start_ns = event.window_start.nanoseconds(); - const auto window_end_ns = event.window_end.nanoseconds(); - - for (const auto & data : bag_result.synchronized_data_list) { - const auto data_time_ns = data->timestamp.nanoseconds(); - if (data_time_ns >= window_start_ns && data_time_ns <= window_end_ns) { - trajectories_in_window.push_back(data); - } - } - - if (trajectories_in_window.empty()) { - RCLCPP_WARN( - logger_, "OR event %zu: No trajectories found in window [%.3f, %.3f]", event_metrics.event_id, - event.window_start.seconds(), event.window_end.seconds()); - return event_metrics; - } - - RCLCPP_INFO( - logger_, "OR event %zu: Found %zu trajectory predictions in window", event_metrics.event_id, - trajectories_in_window.size()); - - // Evaluate each trajectory prediction in the window - // Use ALL bag data for GT interpolation - std::vector trajectory_metrics_list; - - for (const auto & trajectory_data : trajectories_in_window) { - if (!trajectory_data->trajectory || trajectory_data->trajectory->points.empty()) { - continue; - } - - // Pass ALL bag data for GT interpolation (not just window) - auto traj_metrics = - evaluate_trajectory_prediction(trajectory_data, bag_result.synchronized_data_list, event); - trajectory_metrics_list.push_back(traj_metrics); - } - - // Generate debug visualizations if enabled - if (enable_debug_visualization_) { - for (size_t i = 0; i < trajectories_in_window.size() && i < trajectory_metrics_list.size(); - ++i) { - const auto & traj_data = trajectories_in_window[i]; - const auto & traj_metrics = trajectory_metrics_list[i]; - - if (traj_metrics.num_points > 0 && !traj_metrics.ground_truth_trajectory.points.empty()) { - // Extract GT poses from stored trajectory - std::vector gt_poses; - for (const auto & pt : traj_metrics.ground_truth_trajectory.points) { - gt_poses.push_back(pt.pose); - } - - generate_debug_visualization(traj_data, gt_poses, event, traj_metrics, debug_output_dir_); - } - } - } - - // Calculate aggregate metrics for this event - event_metrics = calculate_event_metrics(trajectory_metrics_list, event); - - // Save to bag if writer provided - if (bag_writer) { - save_event_metrics_to_bag(event_metrics, event, *bag_writer); - } - - return event_metrics; -} - -std::vector> ORSceneEvaluator::load_data_for_or_window( - const OREvent & event, const std::string & bag_path, const TopicNames & topic_names) -{ - // For now, we need to process the entire bag to get all data for GT interpolation - // GT generation requires data beyond the evaluation window (for trajectory horizon) - // TODO(go-sakayori): Optimize to only read specific time range from bag - - RCLCPP_INFO(logger_, "Loading bag data for GT generation (processing entire bag)..."); - auto bag_result = process_bag_common(bag_path, nullptr, topic_names); - - RCLCPP_INFO( - logger_, "Total synchronized data points in bag: %zu", - bag_result.synchronized_data_list.size()); - - // Filter to get trajectory predictions in OR event window only - // But keep ALL data for GT interpolation (trajectories need future kinematic states) - std::vector> window_data; - const auto window_start_ns = event.window_start.nanoseconds(); - const auto window_end_ns = event.window_end.nanoseconds(); - - for (const auto & data : bag_result.synchronized_data_list) { - const auto data_time_ns = data->timestamp.nanoseconds(); - if (data_time_ns >= window_start_ns && data_time_ns <= window_end_ns) { - window_data.push_back(data); - } - } - - RCLCPP_INFO( - logger_, "Filtered %zu trajectory predictions in window [%.3f, %.3f]", window_data.size(), - event.window_start.seconds(), event.window_end.seconds()); - - return window_data; -} - -ORTrajectoryMetrics ORSceneEvaluator::evaluate_trajectory_prediction( - const std::shared_ptr & trajectory_data, - const std::vector> & all_data_for_gt, const OREvent & event) -{ - ORTrajectoryMetrics metrics; - metrics.prediction_time = trajectory_data->timestamp; - - // Calculate time relative to OR (in seconds, preserving sign) - const double time_diff_sec = - (trajectory_data->timestamp.nanoseconds() - event.timestamp.nanoseconds()) / 1e9; - metrics.time_relative_to_or_sec = time_diff_sec; // Negative if before OR - - const auto & original_trajectory = *(trajectory_data->trajectory); - metrics.num_points_original = original_trajectory.points.size(); - - if (!original_trajectory.points.empty()) { - metrics.trajectory_duration_original = - rclcpp::Duration(original_trajectory.points.back().time_from_start).seconds(); - } else { - metrics.trajectory_duration_original = 0.0; - } - - // Get GT data time range - if (all_data_for_gt.empty()) { - RCLCPP_WARN(logger_, "No GT data available"); - metrics.num_points = 0; - metrics.trajectory_duration = 0.0; - metrics.gt_coverage_ratio = 0.0; - return metrics; - } - - const rclcpp::Time gt_start = all_data_for_gt.front()->timestamp; - const rclcpp::Time gt_end = all_data_for_gt.back()->timestamp; - - // Truncate trajectory to available GT range - auto [truncated_traj, coverage] = truncate_trajectory_to_gt_range( - original_trajectory, trajectory_data->timestamp, gt_start, gt_end); - - metrics.gt_coverage_ratio = coverage; - - if (truncated_traj.points.empty()) { - RCLCPP_WARN( - logger_, "Trajectory at t=%.3f completely outside GT range [%.3f, %.3f]", - metrics.prediction_time.seconds(), gt_start.seconds(), gt_end.seconds()); - metrics.num_points = 0; - metrics.trajectory_duration = 0.0; - return metrics; - } - - RCLCPP_DEBUG( - logger_, "Truncated trajectory from %zu to %zu points (%.1f%% coverage)", - metrics.num_points_original, truncated_traj.points.size(), coverage * 100.0); - - // Create temporary SynchronizedData with truncated trajectory - auto truncated_data = std::make_shared(*trajectory_data); - truncated_data->trajectory = - std::make_shared(truncated_traj); - - // Generate ground truth for truncated trajectory - auto ground_truth_opt = - open_loop_evaluator_->generate_ground_truth_trajectory(truncated_data, all_data_for_gt); - - if (!ground_truth_opt.has_value()) { - RCLCPP_WARN( - logger_, "Failed to generate GT for truncated trajectory at t=%.3f", - metrics.prediction_time.seconds()); - metrics.num_points = 0; - metrics.trajectory_duration = 0.0; - return metrics; - } - - // Create EvaluationData structure for OpenLoopEvaluator - OpenLoopEvaluator::EvaluationData eval_data; - eval_data.synchronized_data = truncated_data; - eval_data.ground_truth_trajectory = ground_truth_opt.value(); - - // Store for visualization - metrics.predicted_trajectory = truncated_traj; - metrics.ground_truth_trajectory = ground_truth_opt.value(); - - // Evaluate trajectory using OpenLoopEvaluator - auto open_loop_metrics = open_loop_evaluator_->evaluate_trajectory(eval_data); - - // Convert OpenLoopTrajectoryMetrics to ORTrajectoryMetrics - metrics.num_points = open_loop_metrics.num_points; - metrics.trajectory_duration = open_loop_metrics.trajectory_duration; - - // Extract ADE/FDE - if (!open_loop_metrics.ade.empty()) { - metrics.ade = open_loop_metrics.ade.back(); // Final ADE (average over all points) - } else { - metrics.ade = 0.0; - } - - if (!open_loop_metrics.displacement_errors.empty()) { - metrics.fde = open_loop_metrics.displacement_errors.back(); // Final point error - } else { - metrics.fde = 0.0; - } - - // Lateral/longitudinal deviation - if (!open_loop_metrics.lateral_deviations.empty()) { - metrics.mean_lateral_deviation = std::accumulate( - open_loop_metrics.lateral_deviations.begin(), - open_loop_metrics.lateral_deviations.end(), 0.0) / - open_loop_metrics.lateral_deviations.size(); - metrics.max_lateral_deviation = *std::max_element( - open_loop_metrics.lateral_deviations.begin(), open_loop_metrics.lateral_deviations.end()); - } else { - metrics.mean_lateral_deviation = 0.0; - metrics.max_lateral_deviation = 0.0; - } - - if (!open_loop_metrics.longitudinal_deviations.empty()) { - metrics.mean_longitudinal_deviation = std::accumulate( - open_loop_metrics.longitudinal_deviations.begin(), - open_loop_metrics.longitudinal_deviations.end(), 0.0) / - open_loop_metrics.longitudinal_deviations.size(); - } else { - metrics.mean_longitudinal_deviation = 0.0; - } - - // TTC - if (!open_loop_metrics.ttc.empty()) { - metrics.min_ttc = *std::min_element(open_loop_metrics.ttc.begin(), open_loop_metrics.ttc.end()); - } else { - metrics.min_ttc = std::numeric_limits::max(); - } - - // Evaluate success criteria if enabled - if (success_criteria_.enabled) { - metrics.meets_success_criteria = evaluate_success_criteria(metrics, metrics.failure_reasons); - } else { - metrics.meets_success_criteria = false; // Not checked - } - - return metrics; -} - -bool ORSceneEvaluator::evaluate_success_criteria( - const ORTrajectoryMetrics & metrics, std::vector & failure_reasons) const -{ - failure_reasons.clear(); - - if (!success_criteria_.enabled) { - return false; - } - - bool passes = true; - - if (metrics.ade > success_criteria_.max_ade) { - failure_reasons.push_back( - "ADE too high: " + std::to_string(metrics.ade) + " > " + - std::to_string(success_criteria_.max_ade)); - passes = false; - } - - if (metrics.fde > success_criteria_.max_fde) { - failure_reasons.push_back( - "FDE too high: " + std::to_string(metrics.fde) + " > " + - std::to_string(success_criteria_.max_fde)); - passes = false; - } - - if (std::abs(metrics.mean_lateral_deviation) > success_criteria_.max_lateral_deviation) { - failure_reasons.push_back( - "Lateral deviation too high: " + std::to_string(std::abs(metrics.mean_lateral_deviation)) + - " > " + std::to_string(success_criteria_.max_lateral_deviation)); - passes = false; - } - - if (metrics.min_ttc < success_criteria_.min_ttc) { - failure_reasons.push_back( - "TTC too low: " + std::to_string(metrics.min_ttc) + " < " + - std::to_string(success_criteria_.min_ttc)); - passes = false; - } - - return passes; -} - -OREventMetrics ORSceneEvaluator::calculate_event_metrics( - const std::vector & trajectory_metrics, const OREvent & event) const -{ - OREventMetrics event_metrics; - event_metrics.event_id = event_metrics_list_.size(); - event_metrics.or_timestamp = event.timestamp; - event_metrics.window_start = event.window_start; - event_metrics.window_end = event.window_end; - event_metrics.vehicle_speed_at_or = event.vehicle_speed_mps; - event_metrics.trajectory_metrics = trajectory_metrics; - event_metrics.total_predictions = trajectory_metrics.size(); - - // Count predictions before/after OR - event_metrics.predictions_before_or = std::count_if( - trajectory_metrics.begin(), trajectory_metrics.end(), - [](const auto & m) { return m.time_relative_to_or_sec < 0.0; }); - event_metrics.predictions_after_or = - event_metrics.total_predictions - event_metrics.predictions_before_or; - - if (trajectory_metrics.empty()) { - return event_metrics; - } - - // Collect values for statistics - std::vector ade_values, fde_values, lateral_dev_values, ttc_values; - for (const auto & m : trajectory_metrics) { - ade_values.push_back(m.ade); - fde_values.push_back(m.fde); - lateral_dev_values.push_back(std::abs(m.mean_lateral_deviation)); - if (std::isfinite(m.min_ttc)) { - ttc_values.push_back(m.min_ttc); - } - } - - // ADE statistics - auto ade_stats = calculate_statistics(ade_values); - event_metrics.mean_ade = ade_stats.mean; - event_metrics.std_ade = ade_stats.std_dev; - event_metrics.min_ade = ade_stats.min_val; - event_metrics.max_ade = ade_stats.max_val; - - // FDE statistics - auto fde_stats = calculate_statistics(fde_values); - event_metrics.mean_fde = fde_stats.mean; - event_metrics.std_fde = fde_stats.std_dev; - event_metrics.min_fde = fde_stats.min_val; - event_metrics.max_fde = fde_stats.max_val; - - // Lateral deviation statistics - auto lateral_stats = calculate_statistics(lateral_dev_values); - event_metrics.mean_lateral_deviation = lateral_stats.mean; - event_metrics.max_lateral_deviation = lateral_stats.max_val; - - // TTC statistics - if (!ttc_values.empty()) { - auto ttc_stats = calculate_statistics(ttc_values); - event_metrics.mean_min_ttc = ttc_stats.mean; - event_metrics.worst_ttc = ttc_stats.min_val; - } else { - event_metrics.mean_min_ttc = std::numeric_limits::max(); - event_metrics.worst_ttc = std::numeric_limits::max(); - } - - // Success criteria results - if (success_criteria_.enabled) { - event_metrics.predictions_meeting_criteria = std::count_if( - trajectory_metrics.begin(), trajectory_metrics.end(), - [](const auto & m) { return m.meets_success_criteria; }); - event_metrics.criteria_success_rate = - static_cast(event_metrics.predictions_meeting_criteria) / - event_metrics.total_predictions; - } else { - event_metrics.predictions_meeting_criteria = 0; - event_metrics.criteria_success_rate = 0.0; - } - - return event_metrics; -} - -void ORSceneEvaluator::calculate_summary() -{ - summary_ = ORSceneSummary{}; - summary_.total_or_events = or_events_.size(); - summary_.events_with_valid_predictions = std::count_if( - event_metrics_list_.begin(), event_metrics_list_.end(), - [](const auto & e) { return e.total_predictions > 0; }); - - if (event_metrics_list_.empty()) { - return; - } - - // Collect values across all events - std::vector ade_values, fde_values, lateral_dev_values, ttc_values; - double total_predictions = 0.0; - double total_duration = 0.0; - - for (const auto & event_metrics : event_metrics_list_) { - if (event_metrics.total_predictions == 0) continue; - - ade_values.push_back(event_metrics.mean_ade); - fde_values.push_back(event_metrics.mean_fde); - lateral_dev_values.push_back(event_metrics.mean_lateral_deviation); - if (std::isfinite(event_metrics.mean_min_ttc)) { - ttc_values.push_back(event_metrics.mean_min_ttc); - } - - total_predictions += event_metrics.total_predictions; - total_duration += (event_metrics.window_end - event_metrics.window_start).seconds(); - } - - // ADE statistics - if (!ade_values.empty()) { - auto ade_stats = calculate_statistics(ade_values); - summary_.mean_ade_all_events = ade_stats.mean; - summary_.std_ade_all_events = ade_stats.std_dev; - summary_.min_ade_all_events = ade_stats.min_val; - summary_.max_ade_all_events = ade_stats.max_val; - } - - // FDE statistics - if (!fde_values.empty()) { - auto fde_stats = calculate_statistics(fde_values); - summary_.mean_fde_all_events = fde_stats.mean; - summary_.std_fde_all_events = fde_stats.std_dev; - summary_.min_fde_all_events = fde_stats.min_val; - summary_.max_fde_all_events = fde_stats.max_val; - } - - // Lateral deviation statistics - if (!lateral_dev_values.empty()) { - auto lateral_stats = calculate_statistics(lateral_dev_values); - summary_.mean_lateral_deviation_all_events = lateral_stats.mean; - summary_.std_lateral_deviation_all_events = lateral_stats.std_dev; - summary_.max_lateral_deviation_all_events = lateral_stats.max_val; - } - - // TTC statistics - if (!ttc_values.empty()) { - auto ttc_stats = calculate_statistics(ttc_values); - summary_.mean_min_ttc_all_events = ttc_stats.mean; - summary_.worst_ttc_all_events = ttc_stats.min_val; - } else { - summary_.mean_min_ttc_all_events = std::numeric_limits::max(); - summary_.worst_ttc_all_events = std::numeric_limits::max(); - } - - // Coverage statistics - summary_.mean_predictions_per_event = - summary_.events_with_valid_predictions > 0 - ? total_predictions / summary_.events_with_valid_predictions - : 0.0; - summary_.total_evaluation_duration = total_duration; - - // Success criteria results - if (success_criteria_.enabled) { - summary_.events_meeting_criteria = std::count_if( - event_metrics_list_.begin(), event_metrics_list_.end(), - [](const auto & e) { return e.predictions_meeting_criteria > 0; }); - summary_.criteria_success_rate = - static_cast(summary_.events_meeting_criteria) / summary_.total_or_events; - } else { - summary_.events_meeting_criteria = 0; - summary_.criteria_success_rate = 0.0; - } -} - -nlohmann::json ORSceneEvaluator::get_summary_as_json() const -{ - nlohmann::json j; - - j["total_or_events"] = summary_.total_or_events; - j["events_with_valid_predictions"] = summary_.events_with_valid_predictions; - - j["ade"]["mean"] = summary_.mean_ade_all_events; - j["ade"]["std"] = summary_.std_ade_all_events; - j["ade"]["min"] = summary_.min_ade_all_events; - j["ade"]["max"] = summary_.max_ade_all_events; - - j["fde"]["mean"] = summary_.mean_fde_all_events; - j["fde"]["std"] = summary_.std_fde_all_events; - j["fde"]["min"] = summary_.min_fde_all_events; - j["fde"]["max"] = summary_.max_fde_all_events; - - j["lateral_deviation"]["mean"] = summary_.mean_lateral_deviation_all_events; - j["lateral_deviation"]["std"] = summary_.std_lateral_deviation_all_events; - j["lateral_deviation"]["max"] = summary_.max_lateral_deviation_all_events; - - j["ttc"]["mean_min_ttc"] = summary_.mean_min_ttc_all_events; - j["ttc"]["worst_ttc"] = summary_.worst_ttc_all_events; - - j["mean_predictions_per_event"] = summary_.mean_predictions_per_event; - j["total_evaluation_duration_sec"] = summary_.total_evaluation_duration; - - if (success_criteria_.enabled) { - j["success_criteria"]["enabled"] = true; - j["success_criteria"]["events_meeting_criteria"] = summary_.events_meeting_criteria; - j["success_criteria"]["criteria_success_rate"] = summary_.criteria_success_rate; - } else { - j["success_criteria"]["enabled"] = false; - } - - return j; -} - -nlohmann::json ORSceneEvaluator::get_detailed_results_as_json() const -{ - nlohmann::json j; - - j["evaluation_mode"] = "or_scene"; - j["time_window_sec"] = time_window_sec_; - j["summary"] = get_summary_as_json(); - - nlohmann::json events_array = nlohmann::json::array(); - for (const auto & event_metrics : event_metrics_list_) { - nlohmann::json event_json; - - event_json["event_id"] = event_metrics.event_id; - event_json["or_timestamp"] = event_metrics.or_timestamp.seconds(); - event_json["window_start"] = event_metrics.window_start.seconds(); - event_json["window_end"] = event_metrics.window_end.seconds(); - event_json["vehicle_speed_at_or"] = event_metrics.vehicle_speed_at_or; - - event_json["total_predictions"] = event_metrics.total_predictions; - event_json["predictions_before_or"] = event_metrics.predictions_before_or; - event_json["predictions_after_or"] = event_metrics.predictions_after_or; - - event_json["mean_ade"] = event_metrics.mean_ade; - event_json["std_ade"] = event_metrics.std_ade; - event_json["min_ade"] = event_metrics.min_ade; - event_json["max_ade"] = event_metrics.max_ade; - - event_json["mean_fde"] = event_metrics.mean_fde; - event_json["std_fde"] = event_metrics.std_fde; - event_json["min_fde"] = event_metrics.min_fde; - event_json["max_fde"] = event_metrics.max_fde; - - event_json["mean_lateral_deviation"] = event_metrics.mean_lateral_deviation; - event_json["max_lateral_deviation"] = event_metrics.max_lateral_deviation; - - event_json["mean_min_ttc"] = event_metrics.mean_min_ttc; - event_json["worst_ttc"] = event_metrics.worst_ttc; - - if (success_criteria_.enabled) { - event_json["predictions_meeting_criteria"] = event_metrics.predictions_meeting_criteria; - event_json["criteria_success_rate"] = event_metrics.criteria_success_rate; - } - - // Per-trajectory details - nlohmann::json predictions_array = nlohmann::json::array(); - for (const auto & traj_metrics : event_metrics.trajectory_metrics) { - nlohmann::json traj_json; - - traj_json["prediction_time"] = traj_metrics.prediction_time.seconds(); - traj_json["time_relative_to_or"] = traj_metrics.time_relative_to_or_sec; - - traj_json["ade"] = traj_metrics.ade; - traj_json["fde"] = traj_metrics.fde; - traj_json["mean_lateral_deviation"] = traj_metrics.mean_lateral_deviation; - traj_json["max_lateral_deviation"] = traj_metrics.max_lateral_deviation; - traj_json["mean_longitudinal_deviation"] = traj_metrics.mean_longitudinal_deviation; - traj_json["min_ttc"] = traj_metrics.min_ttc; - - traj_json["num_points"] = traj_metrics.num_points; - traj_json["num_points_original"] = traj_metrics.num_points_original; - traj_json["trajectory_duration"] = traj_metrics.trajectory_duration; - traj_json["trajectory_duration_original"] = traj_metrics.trajectory_duration_original; - traj_json["gt_coverage_ratio"] = traj_metrics.gt_coverage_ratio; - - if (success_criteria_.enabled) { - traj_json["meets_criteria"] = traj_metrics.meets_success_criteria; - if (!traj_metrics.failure_reasons.empty()) { - traj_json["failure_reasons"] = traj_metrics.failure_reasons; - } - } - - predictions_array.push_back(traj_json); - } - event_json["predictions"] = predictions_array; - - events_array.push_back(event_json); - } - j["or_events"] = events_array; - - return j; -} - -std::vector> ORSceneEvaluator::get_result_topics() const -{ - std::string prefix = metric_topic_prefix_.empty() ? "" : "/" + metric_topic_prefix_; - return { - {prefix + "/or_scene/event_markers", "visualization_msgs/msg/MarkerArray"}, - {prefix + "/or_scene/ade", "std_msgs/msg/Float64"}, - {prefix + "/or_scene/fde", "std_msgs/msg/Float64"}, - {prefix + "/or_scene/lateral_deviation", "std_msgs/msg/Float64"}, - {prefix + "/or_scene/ttc", "std_msgs/msg/Float64"}, - {prefix + "/or_scene/prediction_success", "std_msgs/msg/Bool"}, - {prefix + "/or_scene/predicted_trajectory", "autoware_planning_msgs/msg/Trajectory"}, - {prefix + "/or_scene/ground_truth_trajectory", "autoware_planning_msgs/msg/Trajectory"}, - {"/tf", "tf2_msgs/msg/TFMessage"}, - {"/tf_static", "tf2_msgs/msg/TFMessage"}}; -} - -void ORSceneEvaluator::save_event_metrics_to_bag( - const OREventMetrics & event_metrics, const OREvent & event, rosbag2_cpp::Writer & bag_writer) -{ - std::string prefix = metric_topic_prefix_.empty() ? "" : "/" + metric_topic_prefix_; - - // Create markers for OR event - auto markers = create_or_event_markers(event, event_metrics); - const rclcpp::Time marker_time = event.timestamp; - bag_writer.write(markers, prefix + "/or_scene/event_markers", marker_time); - - // Write per-trajectory metrics - for (const auto & traj_metrics : event_metrics.trajectory_metrics) { - const rclcpp::Time traj_time = traj_metrics.prediction_time; - - // ADE - std_msgs::msg::Float64 ade_msg; - ade_msg.data = traj_metrics.ade; - bag_writer.write(ade_msg, prefix + "/or_scene/ade", traj_time); - - // FDE - std_msgs::msg::Float64 fde_msg; - fde_msg.data = traj_metrics.fde; - bag_writer.write(fde_msg, prefix + "/or_scene/fde", traj_time); - - // Lateral deviation - std_msgs::msg::Float64 lateral_msg; - lateral_msg.data = traj_metrics.mean_lateral_deviation; - bag_writer.write(lateral_msg, prefix + "/or_scene/lateral_deviation", traj_time); - - // TTC - std_msgs::msg::Float64 ttc_msg; - ttc_msg.data = traj_metrics.min_ttc; - bag_writer.write(ttc_msg, prefix + "/or_scene/ttc", traj_time); - - // Success flag (if criteria enabled) - if (success_criteria_.enabled) { - std_msgs::msg::Bool success_msg; - success_msg.data = traj_metrics.meets_success_criteria; - bag_writer.write(success_msg, prefix + "/or_scene/prediction_success", traj_time); - } - } -} - -visualization_msgs::msg::MarkerArray ORSceneEvaluator::create_or_event_markers( - const OREvent & /* event */, const OREventMetrics & /* event_metrics */) const -{ - visualization_msgs::msg::MarkerArray marker_array; - - // TODO(go-sakayori): Create visualization markers for OR event - // - Marker at OR location - // - Text showing event_id, metrics summary - // - Timeline markers showing evaluation window - - return marker_array; -} - -std::pair -ORSceneEvaluator::truncate_trajectory_to_gt_range( - const autoware_planning_msgs::msg::Trajectory & trajectory, - const rclcpp::Time & trajectory_start_time, const rclcpp::Time & gt_data_start, - const rclcpp::Time & gt_data_end) const -{ - autoware_planning_msgs::msg::Trajectory truncated; - truncated.header = trajectory.header; - - const size_t original_size = trajectory.points.size(); - - if (original_size == 0) { - return {truncated, 0.0}; - } - - // Find points within GT data range - for (const auto & pt : trajectory.points) { - const rclcpp::Time point_time = trajectory_start_time + rclcpp::Duration(pt.time_from_start); - - // Check if this point is within GT data range (use nanoseconds to avoid clock type mismatch) - const int64_t point_ns = point_time.nanoseconds(); - const int64_t gt_start_ns = gt_data_start.nanoseconds(); - const int64_t gt_end_ns = gt_data_end.nanoseconds(); - - if (point_ns >= gt_start_ns && point_ns <= gt_end_ns) { - truncated.points.push_back(pt); - } - } - - const double coverage = - truncated.points.empty() ? 0.0 : static_cast(truncated.points.size()) / original_size; - - return {truncated, coverage}; -} - -void ORSceneEvaluator::generate_debug_visualization( - const std::shared_ptr & trajectory_data, - const std::vector & ground_truth_poses, const OREvent & event, - const ORTrajectoryMetrics & metrics, const std::string & output_dir) -{ - RCLCPP_INFO(logger_, "Starting debug visualization generation..."); - - // Prepare data for Python visualization script - nlohmann::json viz_data; - - // Predicted trajectory (use truncated version from metrics) - nlohmann::json pred_array = nlohmann::json::array(); - for (const auto & pt : metrics.predicted_trajectory.points) { - nlohmann::json pose_json; - pose_json["x"] = pt.pose.position.x; - pose_json["y"] = pt.pose.position.y; - pose_json["z"] = pt.pose.position.z; - pred_array.push_back(pose_json); - } - viz_data["predicted_trajectory"] = pred_array; - - // Ground truth trajectory - nlohmann::json gt_array = nlohmann::json::array(); - for (const auto & pose : ground_truth_poses) { - nlohmann::json pose_json; - pose_json["x"] = pose.position.x; - pose_json["y"] = pose.position.y; - pose_json["z"] = pose.position.z; - gt_array.push_back(pose_json); - } - viz_data["ground_truth_trajectory"] = gt_array; - - // Metrics - viz_data["metrics"]["ade"] = metrics.ade; - viz_data["metrics"]["fde"] = metrics.fde; - viz_data["metrics"]["mean_lateral_deviation"] = metrics.mean_lateral_deviation; - viz_data["metrics"]["min_ttc"] = metrics.min_ttc; - viz_data["metrics"]["num_points"] = metrics.num_points; - viz_data["metrics"]["trajectory_duration"] = metrics.trajectory_duration; - viz_data["metrics"]["time_relative_to_or"] = metrics.time_relative_to_or_sec; - - // Event info - viz_data["event_info"]["event_id"] = event_metrics_list_.size(); - viz_data["event_info"]["or_timestamp"] = event.timestamp.seconds(); - viz_data["event_info"]["vehicle_speed_at_or"] = event.vehicle_speed_mps; - - // Vehicle position at OR (from event struct, not trajectory data) - viz_data["event_info"]["vehicle_x_at_or"] = event.vehicle_x_at_or; - viz_data["event_info"]["vehicle_y_at_or"] = event.vehicle_y_at_or; - - viz_data["event_info"]["bag_name"] = "rosbag"; - - // Add map path for lanelet visualization (optional) - if (!map_path_.empty()) { - viz_data["map_path"] = map_path_; - } - - // Add objects if available - if (trajectory_data->objects && !trajectory_data->objects->objects.empty()) { - nlohmann::json objects_array = nlohmann::json::array(); - - for (const auto & obj : trajectory_data->objects->objects) { - nlohmann::json obj_json; - - // Position and orientation - const auto & pose = obj.kinematics.initial_pose_with_covariance.pose; - obj_json["x"] = pose.position.x; - obj_json["y"] = pose.position.y; - obj_json["orientation"]["x"] = pose.orientation.x; - obj_json["orientation"]["y"] = pose.orientation.y; - obj_json["orientation"]["z"] = pose.orientation.z; - obj_json["orientation"]["w"] = pose.orientation.w; - - // Dimensions - obj_json["length"] = obj.shape.dimensions.x; - obj_json["width"] = obj.shape.dimensions.y; - obj_json["height"] = obj.shape.dimensions.z; - - // Shape type and footprint - obj_json["shape_type"] = obj.shape.type; - - // If polygon footprint is available, use it - if (!obj.shape.footprint.points.empty()) { - nlohmann::json footprint_array = nlohmann::json::array(); - for (const auto & pt : obj.shape.footprint.points) { - nlohmann::json pt_json; - pt_json["x"] = pt.x; - pt_json["y"] = pt.y; - footprint_array.push_back(pt_json); - } - obj_json["footprint"] = footprint_array; - } - - // Classification - if (!obj.classification.empty()) { - obj_json["class_label"] = obj.classification[0].label; - } - - objects_array.push_back(obj_json); - } - - viz_data["objects"] = objects_array; - } else { - viz_data["objects"] = nlohmann::json::array(); - } - - // Expand ~ in output_dir - std::string expanded_dir = output_dir; - if (expanded_dir.substr(0, 2) == "~/") { - const char * home = std::getenv("HOME"); - if (home) { - expanded_dir = std::string(home) + expanded_dir.substr(1); - } - } - - // Create temporary JSON file - const std::string temp_json = expanded_dir + "/temp_viz_data.json"; - std::ofstream temp_file(temp_json); - if (!temp_file.is_open()) { - RCLCPP_ERROR(logger_, "Failed to create temp visualization data file: %s", temp_json.c_str()); - return; - } - temp_file << viz_data.dump(2); - temp_file.close(); - - // Generate output filename - const int64_t pred_time_ms = static_cast(metrics.prediction_time.seconds() * 1000); - const std::string output_image = expanded_dir + "/or_event_" + - std::to_string(event_metrics_list_.size()) + "_pred_" + - std::to_string(pred_time_ms) + ".png"; - - // Call Python visualization script - // Use ament_index to find the installed script - std::string script_path; - try { - const std::string share_dir = - ament_index_cpp::get_package_share_directory("autoware_planning_data_analyzer"); - script_path = share_dir + "/scripts/generate_or_visualization.py"; - } catch (const std::exception & e) { - RCLCPP_ERROR(logger_, "Failed to find package share directory: %s", e.what()); - return; - } - - if (!std::filesystem::exists(script_path)) { - RCLCPP_ERROR(logger_, "Visualization script not found: %s", script_path.c_str()); - return; - } - - const std::string cmd = "python3 " + script_path + " " + temp_json + " " + output_image + " 2>&1"; - - const int result = std::system(cmd.c_str()); - - if (result == 0) { - RCLCPP_INFO(logger_, "Generated debug visualization: %s", output_image.c_str()); - } else { - RCLCPP_WARN(logger_, "Failed to generate visualization (return code: %d)", result); - } - - // Keep temp file for debugging (don't clean up) - // std::filesystem::remove(temp_json); -} - -} // namespace autoware::planning_data_analyzer diff --git a/planning/autoware_planning_data_analyzer/src/or_scene_evaluator.hpp b/planning/autoware_planning_data_analyzer/src/or_scene_evaluator.hpp deleted file mode 100644 index dfa952f70..000000000 --- a/planning/autoware_planning_data_analyzer/src/or_scene_evaluator.hpp +++ /dev/null @@ -1,251 +0,0 @@ -// Copyright 2025 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. - -#ifndef OR_SCENE_EVALUATOR_HPP_ -#define OR_SCENE_EVALUATOR_HPP_ - -#include "base_evaluator.hpp" -#include "open_loop_evaluator.hpp" -#include "or_event_extractor.hpp" -#include "or_scene_structs.hpp" - -#include -#include -#include - -#include - -#include -#include -#include -#include - -namespace autoware::planning_data_analyzer -{ - -using autoware::route_handler::RouteHandler; - -/** - * @brief Evaluates trajectory predictions in Override (OR) scenarios - * - * This evaluator performs two-stage evaluation of planning performance - * around detected OR events (when safety driver takes manual control): - * - * Stage 1: Extract OR events from bag (using OREventExtractor) - * Stage 2: For each OR event, evaluate trajectory predictions in time window - * - * Time window: [OR - t, OR + t] where t is configurable (default: 0.5s) - * - * For each trajectory prediction in the window: - * - Generate ground truth from actual vehicle motion (kinematic_state) - * - Calculate ADE, FDE, lateral deviation, TTC - * - Optionally check against success criteria thresholds - * - * Outputs three levels of detail: - * 1. Per-trajectory metrics (for each prediction in each OR window) - * 2. Per-OR-event summary (aggregated statistics for each OR) - * 3. Overall summary (statistics across all OR events) - */ -class ORSceneEvaluator : public BaseEvaluator -{ -public: - /** - * @brief Constructor - * @param logger ROS logger - * @param route_handler Route handler for map-based calculations - * @param time_window_sec Time window on each side of OR (default: 0.5s) - * @param success_criteria Success criteria thresholds (optional) - * @param enable_debug_visualization Enable debug image generation (default: false) - * @param debug_output_dir Directory for debug images (default: "~/or_scene_debug_images") - */ - ORSceneEvaluator( - rclcpp::Logger logger, std::shared_ptr route_handler, - double time_window_sec = 0.5, const ORSuccessCriteria & success_criteria = ORSuccessCriteria(), - bool enable_debug_visualization = false, - const std::string & debug_output_dir = "~/or_scene_debug_images"); - - // BaseEvaluator interface implementation - void evaluate( - const std::vector> & synchronized_data_list, - rosbag2_cpp::Writer * bag_writer) override; - - std::pair run_evaluation_from_bag( - const std::string & bag_path, rosbag2_cpp::Writer * evaluation_bag_writer, - const TopicNames & topic_names) override; - - nlohmann::json get_summary_as_json() const override; - nlohmann::json get_detailed_results_as_json() const override; - std::vector> get_result_topics() const override; - - /** - * @brief Set path to pre-extracted OR events JSON (skips Stage 1) - * @param json_path Path to or_events.json file - */ - void set_or_events_json_path(const std::string & json_path); - - /** - * @brief Set output path for OR events JSON - * @param json_path Path where or_events.json should be saved - */ - void set_or_events_output_path(const std::string & json_path); - - /** - * @brief Set input bag path for OR extraction (when evaluate_live_trajectories=true) - * @param bag_path Path to input bag containing OR events - */ - void set_input_bag_path(const std::string & bag_path); - - /** - * @brief Set map path for lanelet visualization - * @param map_path Path to lanelet2_map.osm file - */ - void set_map_path(const std::string & map_path); - - /** - * @brief Set metric topic prefix for multi-run collection - * @param prefix Prefix for metric topics (e.g., "model_v1" -> "/model_v1/or_scene/ade") - */ - void set_metric_topic_prefix(const std::string & prefix); - -private: - double time_window_sec_; - ORSuccessCriteria success_criteria_; - bool enable_debug_visualization_; - std::string debug_output_dir_; - std::string map_path_; // Optional: path to lanelet2_map.osm for visualization - std::string metric_topic_prefix_; // Optional: prefix for metric topics (multi-run support) - - std::string or_events_json_input_path_; // Optional: pre-extracted OR events - std::string or_events_json_output_path_; // Where to save extracted OR events - std::string input_bag_path_; // Optional: input bag for OR extraction (LIVE mode) - - std::vector or_events_; - std::vector event_metrics_list_; - ORSceneSummary summary_; - - // Reuse OpenLoopEvaluator for GT generation and comparison - std::unique_ptr open_loop_evaluator_; - - /** - * @brief Stage 1: Extract OR events from bag - * @param bag_path Path to input bag - * @param topic_names Topic names configuration - * @return Vector of OR events - */ - std::vector extract_or_events( - const std::string & bag_path, const TopicNames & topic_names); - - /** - * @brief Stage 2: Evaluate single OR event - * @param event OR event to evaluate - * @param bag_path Path to input bag - * @param bag_writer Output bag writer (optional) - * @param topic_names Topic names configuration - * @return Metrics for this OR event - */ - OREventMetrics evaluate_or_event( - const OREvent & event, const std::string & bag_path, rosbag2_cpp::Writer * bag_writer, - const TopicNames & topic_names); - - /** - * @brief Load synchronized data for OR event time window - * @param event OR event - * @param bag_path Path to input bag - * @param topic_names Topic names configuration - * @return Synchronized data in window [event.window_start, event.window_end] - */ - std::vector> load_data_for_or_window( - const OREvent & event, const std::string & bag_path, const TopicNames & topic_names); - - /** - * @brief Evaluate single trajectory prediction against ground truth - * @param trajectory_data Trajectory to evaluate - * @param all_data_for_gt All synchronized data (for GT interpolation) - * @param event OR event context - * @return Metrics for this trajectory - */ - ORTrajectoryMetrics evaluate_trajectory_prediction( - const std::shared_ptr & trajectory_data, - const std::vector> & all_data_for_gt, const OREvent & event); - - /** - * @brief Truncate trajectory to available GT time range - * @param trajectory Original trajectory - * @param trajectory_start_time Timestamp when trajectory was published - * @param gt_data_start First GT timestamp - * @param gt_data_end Last GT timestamp - * @return Truncated trajectory and coverage ratio - */ - std::pair truncate_trajectory_to_gt_range( - const autoware_planning_msgs::msg::Trajectory & trajectory, - const rclcpp::Time & trajectory_start_time, const rclcpp::Time & gt_data_start, - const rclcpp::Time & gt_data_end) const; - - /** - * @brief Check if trajectory meets success criteria - * @param metrics Trajectory metrics to check - * @param failure_reasons Output vector of failure reasons (if any) - * @return true if all criteria met - */ - bool evaluate_success_criteria( - const ORTrajectoryMetrics & metrics, std::vector & failure_reasons) const; - - /** - * @brief Calculate aggregate statistics for OR event - * @param trajectory_metrics Vector of per-trajectory metrics - * @return Aggregated event metrics - */ - OREventMetrics calculate_event_metrics( - const std::vector & trajectory_metrics, const OREvent & event) const; - - /** - * @brief Calculate overall summary statistics - */ - void calculate_summary(); - - /** - * @brief Save event metrics to output bag - * @param event_metrics Metrics for one OR event - * @param event OR event context - * @param bag_writer Output bag writer - */ - void save_event_metrics_to_bag( - const OREventMetrics & event_metrics, const OREvent & event, rosbag2_cpp::Writer & bag_writer); - - /** - * @brief Generate debug visualization image for trajectory comparison - * @param trajectory_data Predicted trajectory - * @param ground_truth_poses Ground truth poses - * @param event OR event context - * @param metrics Evaluation metrics - * @param output_dir Directory to save image - */ - void generate_debug_visualization( - const std::shared_ptr & trajectory_data, - const std::vector & ground_truth_poses, const OREvent & event, - const ORTrajectoryMetrics & metrics, const std::string & output_dir); - - /** - * @brief Create visualization marker array for OR event - * @param event OR event - * @param event_metrics Metrics for this event - * @return MarkerArray message - */ - visualization_msgs::msg::MarkerArray create_or_event_markers( - const OREvent & event, const OREventMetrics & event_metrics) const; -}; - -} // namespace autoware::planning_data_analyzer - -#endif // OR_SCENE_EVALUATOR_HPP_ diff --git a/planning/autoware_planning_data_analyzer/src/or_scene_structs.hpp b/planning/autoware_planning_data_analyzer/src/or_scene_structs.hpp deleted file mode 100644 index ae096136b..000000000 --- a/planning/autoware_planning_data_analyzer/src/or_scene_structs.hpp +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright 2025 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. - -#ifndef OR_SCENE_STRUCTS_HPP_ -#define OR_SCENE_STRUCTS_HPP_ - -#include - -#include - -#include -#include - -namespace autoware::planning_data_analyzer -{ - -/** - * @brief Represents a detected Override (OR) event from bag data - * - * An OR event occurs when operation mode transitions from AUTONOMOUS to LOCAL, - * indicating the safety driver took manual control of the vehicle. - */ -struct OREvent -{ - rclcpp::Time timestamp; // Timestamp when OR signal was detected - rclcpp::Time window_start; // OR_timestamp - time_window_sec - rclcpp::Time window_end; // OR_timestamp + time_window_sec - rclcpp::Duration time_window = - rclcpp::Duration(0, 0); // Evaluation time window (symmetric around OR) - - double vehicle_speed_mps; // Vehicle speed at OR event - double vehicle_x_at_or; // Vehicle X position at OR event - double vehicle_y_at_or; // Vehicle Y position at OR event - uint8_t prev_mode; // Previous operation mode (AUTONOMOUS = 2) - uint8_t new_mode; // New operation mode (LOCAL = 3) - std::string prev_mode_str; // Human-readable previous mode - std::string new_mode_str; // Human-readable new mode -}; - -/** - * @brief Success criteria thresholds for trajectory evaluation (optional) - * - * These thresholds are disabled by default (like open_loop evaluation). - * When enabled, they classify trajectories as meeting/not meeting criteria. - */ -struct ORSuccessCriteria -{ - bool enabled = false; // Enforce thresholds? - - double max_ade = 1.0; // Maximum allowed ADE (meters) - double max_fde = 1.5; // Maximum allowed FDE (meters) - double max_lateral_deviation = 0.5; // Maximum lateral deviation (meters) - double min_ttc = 3.0; // Minimum required TTC (seconds) -}; - -/** - * @brief Metrics for a single trajectory prediction - * - * Contains evaluation metrics comparing predicted trajectory to ground truth - * generated from actual vehicle motion (kinematic_state poses). - */ -struct ORTrajectoryMetrics -{ - rclcpp::Time prediction_time; // When trajectory was published - double - time_relative_to_or_sec; // prediction_time - OR_timestamp in seconds (negative = before OR) - - // Trajectory quality metrics (reuse from OpenLoopEvaluator) - double ade; // Average Displacement Error (meters) - double fde; // Final Displacement Error (meters) - double mean_lateral_deviation; // Average lateral deviation (meters) - double max_lateral_deviation; // Maximum lateral deviation (meters) - double mean_longitudinal_deviation; // Average longitudinal deviation (meters) - double min_ttc; // Minimum Time To Collision (seconds) - - size_t num_points; // Number of trajectory points evaluated - size_t num_points_original; // Original trajectory length before truncation - double trajectory_duration; // Duration of evaluated trajectory (seconds) - double trajectory_duration_original; // Original trajectory duration before truncation - double gt_coverage_ratio; // Fraction of trajectory with GT data (0.0-1.0) - - // Optional: Success criteria evaluation - bool meets_success_criteria; // Passed all thresholds? - std::vector failure_reasons; // Why it failed (if criteria enabled) - - // For debug visualization (not serialized to JSON) - autoware_planning_msgs::msg::Trajectory predicted_trajectory; // Truncated predicted trajectory - autoware_planning_msgs::msg::Trajectory - ground_truth_trajectory; // GT trajectory for visualization -}; - -/** - * @brief Aggregated metrics for one OR event - * - * Combines metrics from all trajectory predictions in the time window - * around a single OR event. - */ -struct OREventMetrics -{ - size_t event_id; // OR event index (0, 1, 2, ...) - rclcpp::Time or_timestamp; // When OR signal was detected - rclcpp::Time window_start; // Evaluation window start - rclcpp::Time window_end; // Evaluation window end - - double vehicle_speed_at_or; // Speed when OR happened - - std::vector trajectory_metrics; // All predictions in window - - // Aggregate statistics across all predictions - size_t total_predictions; // How many trajectories evaluated - size_t predictions_before_or; // How many were before OR timestamp - size_t predictions_after_or; // How many were after OR timestamp - - // ADE statistics - double mean_ade; // Average ADE across predictions - double std_ade; // Standard deviation of ADE - double min_ade; // Best (lowest) ADE - double max_ade; // Worst (highest) ADE - - // FDE statistics - double mean_fde; // Average FDE across predictions - double std_fde; // Standard deviation of FDE - double min_fde; // Best (lowest) FDE - double max_fde; // Worst (highest) FDE - - // Lateral deviation statistics - double mean_lateral_deviation; // Average lateral deviation - double max_lateral_deviation; // Maximum lateral deviation - - // TTC statistics - double mean_min_ttc; // Average of minimum TTCs - double worst_ttc; // Worst (lowest) TTC in window - - // Optional: Success criteria results - size_t predictions_meeting_criteria; // How many passed thresholds (if enabled) - double criteria_success_rate; // predictions_meeting / total (if enabled) -}; - -/** - * @brief Overall summary statistics across all OR events in the bag - */ -struct ORSceneSummary -{ - // Event counts - size_t total_or_events; // How many OR events detected - size_t events_with_valid_predictions; // OR events with at least 1 trajectory in window - - // ADE statistics across all OR events - double mean_ade_all_events; // Mean ADE across all events - double std_ade_all_events; // Std dev of ADE - double max_ade_all_events; // Worst ADE across events - double min_ade_all_events; // Best ADE across events - - // FDE statistics across all OR events - double mean_fde_all_events; // Mean FDE across all events - double std_fde_all_events; // Std dev of FDE - double max_fde_all_events; // Worst FDE across events - double min_fde_all_events; // Best FDE across events - - // Lateral deviation statistics - double mean_lateral_deviation_all_events; // Mean lateral deviation - double std_lateral_deviation_all_events; // Std dev of lateral deviation - double max_lateral_deviation_all_events; // Maximum lateral deviation - - // TTC statistics - double mean_min_ttc_all_events; // Average of minimum TTCs - double worst_ttc_all_events; // Worst (lowest) TTC seen - - // Timing and coverage statistics - double mean_predictions_per_event; // Average number of trajectories per OR event - double total_evaluation_duration; // Total duration covered by all OR events - - // Optional: Success criteria results (only if enabled) - size_t events_meeting_criteria; // How many events had at least one good prediction - double criteria_success_rate; // events_meeting / total (if enabled) -}; - -} // namespace autoware::planning_data_analyzer - -#endif // OR_SCENE_STRUCTS_HPP_