feat(planning_data_analyzer): add new package (#103)

* feat(planning_data_analyzer): add new package Signed-off-by: satoshi-ota <[email protected]> * style(pre-commit): autofix * refactor: use map Signed-off-by: satoshi-ota <[email protected]> * style(pre-commit): autofix * fix: print Signed-off-by: satoshi-ota <[email protected]> * style(pre-commit): autofix * refactor: small change Signed-off-by: satoshi-ota <[email protected]> * feat: weight grid search Signed-off-by: satoshi-ota <[email protected]> * feat: add time delta Signed-off-by: satoshi-ota <[email protected]> * style(pre-commit): autofix * fix: typo Signed-off-by: satoshi-ota <[email protected]> * fix: ignore spell check for foxglove json Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Sep 3, 2024 · 584be31 · 584be31
1 parent 0084c25
commit 584be31
Show file tree

Hide file tree

Showing 14 changed files with 2,092 additions and 1 deletion.
diff --git a/.cspell-partial.json b/.cspell-partial.json
@@ -3,7 +3,8 @@
     "**/perception/**",
     "sensing/tier4_pcl_extensions/include/**",
     "perception/bytetrack/lib/**",
-    "common/autoware_debug_tools/**"
+    "common/autoware_debug_tools/**",
+    "planning/autoware_planning_data_analyzer/**"
   ],
   "ignoreRegExpList": [],
   "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"]

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 comfortabiilty (manual)",
+          "lineSize": 1.5,
+          "showLine": true
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_score.data[1]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 1.5,
+          "label": "longitudinal comfortabiilty (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 comfortabiilty (manual)",
+          "lineSize": 1.5,
+          "showLine": true
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/behavior_analyzer/system_score.data[0]",
+          "enabled": true,
+          "color": "#f5774d",
+          "lineSize": 1.5,
+          "label": "lateral comfortabiilty (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,22 @@
+/**:
+  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.0]
+      longitudinal_velocities: [0.0]
+      longitudinal_accelerations: [-0.2, -0.1, 0.0, 0.1, 0.2]
+
+    weight:
+      lat_comfortability: 1.0
+      lon_comfortability: 1.0
+      efficiency: 1.0
+      safety: 1.0
+
+    grid_seach:
+      grid: [0.01, 0.1, 0.3, 0.5, 0.8, 1.0]
+      dt: 1.0
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>