feat(planning_data_analyzer): add new package

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Aug 21, 2024 · 51c2274 · 51c2274
1 parent fb68ea4
commit 51c2274
Show file tree

Hide file tree

Showing 14 changed files with 1,888 additions and 0 deletions.
diff --git a/bag2lanelet/scripts/bag2map.py b/bag2lanelet/scripts/bag2map.py
@@ -6,6 +6,7 @@
 
 from bag2way import bag2point_stamped
 import folium
+
 from tools.bag2lanelet.scripts.lanelet_xml import LaneletMap
 
 

diff --git a/planning/autoware_planning_data_analyzer/CMakeLists.txt b/planning/autoware_planning_data_analyzer/CMakeLists.txt
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_planning_data_analyzer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/node.cpp
+  src/data_structs.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::behavior_analyzer::BehaviorAnalyzerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)
diff --git a/planning/autoware_planning_data_analyzer/README.md b/planning/autoware_planning_data_analyzer/README.md
@@ -0,0 +1,20 @@
+# Planning Data Analyzer
+
+<div align="center">
+  <img src="./docs/planning_data_analyzer.png" width=100%>
+</div>
+
+## Usage
+
+```sh
+ros2 launch autoware_planning_data_analyzer behavior_analyzer.launch.xml bag_path:=<ROSBAG>
+```
+
+## Output
+
+| Name                      | Type                                              | Description                                                     |
+| ------------------------- | ------------------------------------------------- | --------------------------------------------------------------- |
+| `~/output/manual_metrics` | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the driver's driving trajectory.        |
+| `~/output/system_metrics` | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the autoware output.                    |
+| `~/output/manual_score`   | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the driver's driving trajectory. |
+| `~/output/system_score`   | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the autoware output.             |
diff --git a/planning/autoware_planning_data_analyzer/config/behavior_analyzer.json b/planning/autoware_planning_data_analyzer/config/behavior_analyzer.json
@@ -0,0 +1,294 @@
+{
+  "configById": {
+    "Plot!2xxq5nc": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_metrics.data[40:59]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "longitudinal jerk (manual)",
+          "lineSize": 4
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_metrics.data[40:59]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 4,
+          "label": "longitudinal jerk (system)"
+        }
+      ],
+      "minYValue": -2,
+      "maxYValue": 2,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "index",
+      "sidebarDimension": 240,
+      "minXValue": 0,
+      "maxXValue": 20
+    },
+    "Plot!2s3vi1i": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_score.data[1]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "longitudinal confortability (manual)",
+          "lineSize": 1.5,
+          "showLine": true
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_score.data[1]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 1.5,
+          "label": "longitudinal confortability (system)"
+        }
+      ],
+      "minYValue": 0,
+      "maxYValue": 1,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "followingViewWidth": 500
+    },
+    "Plot!na5bbq": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_score.data[0]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "lateral confortability (manual)",
+          "lineSize": 1.5,
+          "showLine": true
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_score.data[0]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 1.5,
+          "label": "lateral confortability (system)"
+        }
+      ],
+      "minYValue": 0,
+      "maxYValue": 1,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "followingViewWidth": 500
+    },
+    "Plot!1oow4ne": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_metrics.data[60:79]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "travel_distance (manual)",
+          "lineSize": 4
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_metrics.data[60:79]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 4,
+          "label": "travel_distance (system)"
+        }
+      ],
+      "minYValue": 0,
+      "maxYValue": 150,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "index",
+      "sidebarDimension": 240,
+      "minXValue": 0,
+      "maxXValue": 20
+    },
+    "Plot!3wovlah": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_score.data[2]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "efficiency (manual)",
+          "lineSize": 1.5,
+          "showLine": true
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_score.data[2]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 1.5,
+          "label": "efficiency (system)"
+        }
+      ],
+      "minYValue": 0,
+      "maxYValue": 1,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "followingViewWidth": 500
+    },
+    "Plot!4jh9z4a": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_metrics.data[80:99]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "minimum ttc (manual)",
+          "lineSize": 4
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_metrics.data[80:99]",
+          "enabled": true,
+          "color": "#f5774d",
+          "label": "minimum ttc (system)",
+          "lineSize": 4
+        }
+      ],
+      "minYValue": 0,
+      "maxYValue": 20,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "index",
+      "sidebarDimension": 240,
+      "minXValue": 0,
+      "maxXValue": 20
+    },
+    "Plot!37j6i8x": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_score.data[3]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "safety (manual)",
+          "lineSize": 1.5,
+          "showLine": true
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_score.data[3]",
+          "enabled": true,
+          "color": "#f5774d",
+          "label": "safety (system)"
+        }
+      ],
+      "minYValue": 0,
+      "maxYValue": 1,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "followingViewWidth": 500
+    },
+    "Plot!3qlhcp2": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/manual_metrics.data[0:19]",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "lateral acceleration (manual)",
+          "lineSize": 4
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_metrics.data[0:19]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 4,
+          "label": "lateral acceleration (system)"
+        }
+      ],
+      "minYValue": -2,
+      "maxYValue": 2,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "index",
+      "sidebarDimension": 240,
+      "minXValue": 0,
+      "maxXValue": 20
+    }
+  },
+  "globalVariables": {},
+  "userNodes": {},
+  "playbackConfig": {
+    "speed": 1
+  },
+  "layout": {
+    "first": {
+      "first": {
+        "first": "Plot!2xxq5nc",
+        "second": "Plot!3qlhcp2",
+        "direction": "column"
+      },
+      "second": {
+        "first": "Plot!2s3vi1i",
+        "second": "Plot!na5bbq",
+        "direction": "column"
+      },
+      "direction": "row"
+    },
+    "second": {
+      "first": {
+        "first": "Plot!1oow4ne",
+        "second": "Plot!3wovlah",
+        "direction": "row"
+      },
+      "second": {
+        "first": "Plot!4jh9z4a",
+        "second": "Plot!37j6i8x",
+        "direction": "row"
+      },
+      "direction": "column"
+    },
+    "direction": "column",
+    "splitPercentage": 35.595105672969964
+  }
+}
diff --git a/planning/autoware_planning_data_analyzer/config/behavior_analyzer.param.yaml b/planning/autoware_planning_data_analyzer/config/behavior_analyzer.param.yaml
@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    resample_num: 20
+    time_resolution: 0.5
+
+    target_state:
+      lateral_positions: [-4.5, -2.5, 0.0, 2.5, 4.0]
+      lateral_velocities: [0.0]
+      lateral_accelerations: [0.0]
+      longitudinal_positions: [0.1, 0.5, 1.0]
+      longitudinal_velocities: [0.0, 5.56, 11.1]
+      longitudinal_accelerations: [-0.2, -0.1, 0.0, 0.1, 0.2]
diff --git a/planning/autoware_planning_data_analyzer/docs/planning_data_analyzer.png b/planning/autoware_planning_data_analyzer/docs/planning_data_analyzer.png
diff --git a/planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml b/planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml
@@ -0,0 +1,28 @@
+<launch>
+  <arg name="bag_path" description="bagfile path"/>
+  <arg name="use_sim_time" default="false" description="use_sim_time"/>
+  <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
+
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
+  <arg name="input/acceleration" default="/localization/acceleration"/>
+  <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
+  <arg name="input/objects" default="/perception/object_recognition/objects"/>
+  <arg name="input/map_topic" default="/map/vector_map"/>
+  <arg name="input/route_topic" default="/planning/mission_planning/route"/>
+
+  <group scoped="false">
+    <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+    </include>
+  </group>
+
+  <node_container pkg="rclcpp_components" exec="component_container" name="behavior_analyzer_container" namespace="" output="screen">
+    <composable_node pkg="autoware_planning_data_analyzer" plugin="autoware::behavior_analyzer::BehaviorAnalyzerNode" name="behavior_analyzer">
+      <param name="bag_path" value="$(var bag_path)"/>
+      <param from="$(find-pkg-share autoware_planning_data_analyzer)/config/behavior_analyzer.param.yaml"/>
+    </composable_node>
+
+    <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+  </node_container>
+</launch>
Original file line number	Diff line number	Diff line change
Expand Up		@@ -6,6 +6,7 @@

		from bag2way import bag2point_stamped
		import folium

		from tools.bag2lanelet.scripts.lanelet_xml import LaneletMap


Expand Down