Skip to content

Commit

Permalink
feat(reaction_analyzer): add reaction anaylzer tool to measure end-to…
Browse files Browse the repository at this point in the history 
…-end delay in sudden obstacle braking response (autowarefoundation#5954)

* feat(reaction_analyzer): add reaction anaylzer tool to measure end-to-end delay in sudden obstacle braking response 

Signed-off-by: Berkay Karaman <[email protected]>

* feat: implement message_filters package, clean up

Signed-off-by: Berkay Karaman <[email protected]>

* feat: update style and readme

Signed-off-by: Berkay Karaman <[email protected]>

* feat: add predicted path for the PredictedObject and add publish_only_pointcloud_with_object

Signed-off-by: Berkay Karaman <[email protected]>

* feat: add wrong initialize localization protection, improve code readability

Signed-off-by: Berkay Karaman <[email protected]>

* feat: launch occupancy_grid_map from reaction analyzer's own launch file

Signed-off-by: Berkay Karaman <[email protected]>

* feat: update

Signed-off-by: Berkay Karaman <[email protected]>

* feat: change function names

Signed-off-by: Berkay Karaman <[email protected]>

* feat: update

Signed-off-by: Berkay Karaman <[email protected]>

* feat: improve style, change csv output stringstream

Signed-off-by: Berkay Karaman <[email protected]>

* fix: ci/cd

Signed-off-by: Berkay Karaman <[email protected]>

* feat: update for new sensor setup, fix bug, optimize code, show pipeline latency, update readme

Signed-off-by: Berkay Karaman <[email protected]>

* fix: container die problem

Signed-off-by: Berkay Karaman <[email protected]>

* feat: update stats, check path param, add marker, warn user for wrong reaction_chain

Signed-off-by: Berkay Karaman <[email protected]>

---------

Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
@karishma1911
Berkay Karaman authored and karishma1911 committed Jun 3, 2024
1 parent e4c1b85 commit b864f27
Show file tree
Hide file tree
Showing 19 changed files with 4,026 additions and 0 deletions.
40 changes: 40 additions & 0 deletions tools/reaction_analyzer/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
cmake_minimum_required(VERSION 3.14)
project(reaction_analyzer)

find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(PCL REQUIRED)
find_package(Eigen3 REQUIRED)

ament_auto_add_library(reaction_analyzer SHARED
include/reaction_analyzer_node.hpp
src/reaction_analyzer_node.cpp
include/subscriber.hpp
src/subscriber.cpp
include/topic_publisher.hpp
src/topic_publisher.cpp
include/utils.hpp
src/utils.cpp
)

target_include_directories(reaction_analyzer
SYSTEM PUBLIC
${PCL_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)

target_link_libraries(reaction_analyzer
${PCL_LIBRARIES}
)

rclcpp_components_register_node(reaction_analyzer
PLUGIN "reaction_analyzer::ReactionAnalyzerNode"
EXECUTABLE reaction_analyzer_exe
)

ament_auto_package(
INSTALL_TO_SHARE
param
launch
)
229 changes: 229 additions & 0 deletions tools/reaction_analyzer/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,229 @@
# Reaction Analyzer

## Description

The main purpose of the reaction analyzer package is to measure the reaction times of various nodes within a ROS-based
autonomous driving simulation environment by subscribing to pre-determined topics. This tool is particularly useful for
evaluating the performance of perception, planning, and control pipelines in response to dynamic changes in the
environment, such as sudden obstacles. To be able to measure both control outputs and perception outputs, it was
necessary to divide the node into two running_mode: `planning_control` and `perception_planning`.

![ReactionAnalyzerDesign.png](media%2FReactionAnalyzerDesign.png)

### Planning Control Mode

In this mode, the reaction analyzer creates a dummy publisher for the PredictedObjects and PointCloud2 topics. In the
beginning of the test, it publishes the initial position of the ego vehicle and the goal position to set the test
environment. Then, it spawns a sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to
search reacted messages of the planning and control nodes in the pre-determined topics. When all the topics are reacted,
it calculates the reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes
with `spawn_cmd_time`, and it creates a csv file to store the results.

### Perception Planning Mode

In this mode, the reaction analyzer reads the rosbag files which are recorded from AWSIM, and it creates a topic
publisher for each topic inside the rosbag to replay the rosbag. It reads two rosbag files: `path_bag_without_object`
and `path_bag_with_object`. Firstly, it replays the `path_bag_without_object` to set the initial position of the ego
vehicle and the goal position. After `spawn_time_after_init` seconds , it replays the `path_bag_with_object` to spawn a
sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to search the reacted messages of
the perception and planning nodes in the pre-determined topics. When all the topics are reacted, it calculates the
reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes with `spawn_cmd_time`, and
it creates a csv file to store the results.

#### Point Cloud Publisher Type

To get better analyze for Perception & Sensing pipeline, the reaction analyzer can publish the point cloud messages in 3
different ways: `async_header_sync_publish`, `sync_header_sync_publish` or `async_publish`. (`T` is the period of the
lidar's output)

![PointcloudPublisherType.png](media%2FPointcloudPublisherType.png)

- `async_header_sync_publish`: It publishes the point cloud messages synchronously with asynchronous header times. It
means that each of the lidar's output will be published at the same time, but the headers of the point cloud messages
includes different timestamps because of the phase difference.
- `sync_header_sync_publish`: It publishes the point cloud messages synchronously with synchronous header times. It
means that each of the lidar's output will be published at the same time, and the headers of the point cloud messages
includes the same timestamps.
- `async_publish`: It publishes the point cloud messages asynchronously. It means that each of the lidar's output will
be published at different times.

## Usage

The common parameters you need to define for both running modes are `output_file_path`, `test_iteration`,
and `reaction_chain` list. `output_file_path` is the output file path is the path where the results and statistics
will be stored. `test_iteration` defines how many tests will be performed. The `reaction_chain` list is the list of the
pre-defined topics you want to measure their reaction times.

**IMPORTANT:** Ensure the `reaction_chain` list is correctly defined:

- For `perception_planning` mode, **do not** define `Control` nodes.
- For `planning_control` mode, **do not** define `Perception` nodes.

### Prepared Test Environment

- Download the demonstration test map from the
link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After
downloading,
extract the zip file and use its path as `[MAP_PATH]` in the following commands.

#### Planning Control Mode

- You need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters,
you can start to test with the following command:

```bash
ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=planning_control vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit map_path:=[MAP_PATH]
```

After the command, the `simple_planning_simulator` and the `reaction_analyzer` will be launched. It will automatically
start to test. After the test is completed, the results will be stored in the `output_file_path` you defined.

#### Perception Planning Mode

- Download the rosbag files from the Google Drive
link [here](https://drive.google.com/file/d/1-Qcv7gYfR-usKOjUH8I997w8I4NMhXlX/view?usp=sharing).
- Extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object`
and `path_bag_with_object`.
- You can start to test with the following command:

```bash
ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=perception_planning vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH]
```

After the command, the `e2e_simulator` and the `reaction_analyzer` will be launched. It will automatically start
to test. After the test is completed, the results will be stored in the `output_file_path` you defined.

#### Prepared Test Environment

**Scene without object:**
![sc1-awsim.png](media%2Fsc1-awsim.png)
![sc1-rviz.png](media%2Fsc1-rviz.png)

**Scene object:**
![sc2-awsim.png](media%2Fsc2-awsim.png)
![sc2-rviz.png](media%2Fsc2-rviz.png)

### Custom Test Environment

**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the
parameters.
The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher` (
for `perception_planning` mode) parameters.**

- To set `initialization_pose`, `entity_params`, `goal_pose`:
- Run the AWSIM environment. Tutorial for AWSIM can be found
[here](https://autowarefoundation.github.io/AWSIM/main/GettingStarted/QuickStartDemo/).
- Run the e2e_simulator with the following command:

```bash
ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH]
```

- After EGO is initialized, you can move the ego vehicle to the desired position by using the `SetGoal` button in the
RViz.
- After the EGO stopped in desired position, please localize the dummy obstacle by using the traffic controller. You can
control the traffic by pressing `ESC` button.

**After localize EGO and dummy vehicle, we should write the positions of these entities in the map frame
in `reaction_analyzer.param.yaml`. To achieve this:**

- Get initialization pose from `/awsim/ground_truth/vehicle/pose` topic.
- Get entity params from `/perception/object_recognition/objects` topic.
- Get goal pose from `/planning/mission_planning/goal` topic.

**PS: `initialization_pose` is only valid for `planning_control` mode.**

- After the parameters were noted, we should record the rosbags for the test. To record the rosbags, you can use the
following command:

```bash
ros2 bag record --all
```

- You should record two rosbags: one without the object and one with the object. You can use the traffic controller to
spawn the object in front of the EGO vehicle or remove it.

**NOTE: You should record the rosbags in the same environment with the same position of the EGO vehicle. You don't need
to run Autoware while recording.**

- After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the
paths of the recorded rosbags.

## Results

The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each
pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, `Pipeline Latency`,
and `Total Latency`
for each of the nodes.

- `Node Latency`: The time difference between previous and current node's reaction timestamps. If it is the first node
in the pipeline, it is same as `Pipeline Latency`.
- `Pipeline Latency`: The time difference between published time of the message and pipeline header time.
- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent
timestamp.

## Parameters

| Name | Type | Description |
| ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- |
| `timer_period` | double | [s] Period for the main processing timer. |
| `test_iteration` | int | Number of iterations for the test. |
| `output_file_path` | string | Directory path where test results and statistics will be stored. |
| `spawn_time_after_init` | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode. |
| `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. |
| `poses.initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
| `poses.entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
| `poses.goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. |
| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. |
| `topic_publisher.spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
| `topic_publisher.dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. |
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. |
| `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object` | bool | Default false. Publish only the point cloud messages with the object. |
| `reaction_params.first_brake_params.debug_control_commands` | bool | Debug publish flag. |
| `reaction_params.first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. |
| `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. |
| `reaction_params.first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. |
| `reaction_params.search_zero_vel_params.max_looking_distance` | double | [m] Maximum looking distance for zero velocity on trajectory |
| `reaction_params.search_entity_params.search_radius` | double | [m] Searching radius for spawned entity. Distance between ego pose and entity pose. |
| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. |

## Limitations

- Reaction analyzer has some limitation like `PublisherMessageType`, `SubscriberMessageType` and `ReactionType`. It is
currently supporting following list:

- **Publisher Message Types:**

- `sensor_msgs/msg/PointCloud2`
- `sensor_msgs/msg/CameraInfo`
- `sensor_msgs/msg/Image`
- `geometry_msgs/msg/PoseWithCovarianceStamped`
- `sensor_msgs/msg/Imu`
- `autoware_auto_vehicle_msgs/msg/ControlModeReport`
- `autoware_auto_vehicle_msgs/msg/GearReport`
- `autoware_auto_vehicle_msgs/msg/HazardLightsReport`
- `autoware_auto_vehicle_msgs/msg/SteeringReport`
- `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`
- `autoware_auto_vehicle_msgs/msg/VelocityReport`

- **Subscriber Message Types:**

- `sensor_msgs/msg/PointCloud2`
- `autoware_auto_perception_msgs/msg/DetectedObjects`
- `autoware_auto_perception_msgs/msg/TrackedObjects`
- `autoware_auto_msgs/msg/PredictedObject`
- `autoware_auto_planning_msgs/msg/Trajectory`
- `autoware_auto_control_msgs/msg/AckermannControlCommand`

- **Reaction Types:**
- `FIRST_BRAKE`
- `SEARCH_ZERO_VEL`
- `SEARCH_ENTITY`

## Future improvements

- The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`,
`SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of
the message types.
Loading

0 comments on commit b864f27

Please sign in to comment.