Skip to content

Commit 90f50b7

Browse files
author
Berkay Karaman
authored
feat(reaction_analyzer): add reaction anaylzer tool to measure end-to-end delay in sudden obstacle braking response (#5954)
* feat(reaction_analyzer): add reaction anaylzer tool to measure end-to-end delay in sudden obstacle braking response Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: implement message_filters package, clean up Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: update style and readme Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: add predicted path for the PredictedObject and add publish_only_pointcloud_with_object Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: add wrong initialize localization protection, improve code readability Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: launch occupancy_grid_map from reaction analyzer's own launch file Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: update Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: change function names Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: update Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: improve style, change csv output stringstream Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * fix: ci/cd Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: update for new sensor setup, fix bug, optimize code, show pipeline latency, update readme Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * fix: container die problem Signed-off-by: Berkay Karaman <berkay@leodrive.ai> * feat: update stats, check path param, add marker, warn user for wrong reaction_chain Signed-off-by: Berkay Karaman <berkay@leodrive.ai> --------- Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
1 parent 2e2cf77 commit 90f50b7

19 files changed

+4026
-0
lines changed
+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(reaction_analyzer)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(PCL REQUIRED)
8+
find_package(Eigen3 REQUIRED)
9+
10+
ament_auto_add_library(reaction_analyzer SHARED
11+
include/reaction_analyzer_node.hpp
12+
src/reaction_analyzer_node.cpp
13+
include/subscriber.hpp
14+
src/subscriber.cpp
15+
include/topic_publisher.hpp
16+
src/topic_publisher.cpp
17+
include/utils.hpp
18+
src/utils.cpp
19+
)
20+
21+
target_include_directories(reaction_analyzer
22+
SYSTEM PUBLIC
23+
${PCL_INCLUDE_DIRS}
24+
${EIGEN3_INCLUDE_DIR}
25+
)
26+
27+
target_link_libraries(reaction_analyzer
28+
${PCL_LIBRARIES}
29+
)
30+
31+
rclcpp_components_register_node(reaction_analyzer
32+
PLUGIN "reaction_analyzer::ReactionAnalyzerNode"
33+
EXECUTABLE reaction_analyzer_exe
34+
)
35+
36+
ament_auto_package(
37+
INSTALL_TO_SHARE
38+
param
39+
launch
40+
)

tools/reaction_analyzer/README.md

+229
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,229 @@
1+
# Reaction Analyzer
2+
3+
## Description
4+
5+
The main purpose of the reaction analyzer package is to measure the reaction times of various nodes within a ROS-based
6+
autonomous driving simulation environment by subscribing to pre-determined topics. This tool is particularly useful for
7+
evaluating the performance of perception, planning, and control pipelines in response to dynamic changes in the
8+
environment, such as sudden obstacles. To be able to measure both control outputs and perception outputs, it was
9+
necessary to divide the node into two running_mode: `planning_control` and `perception_planning`.
10+
11+
![ReactionAnalyzerDesign.png](media%2FReactionAnalyzerDesign.png)
12+
13+
### Planning Control Mode
14+
15+
In this mode, the reaction analyzer creates a dummy publisher for the PredictedObjects and PointCloud2 topics. In the
16+
beginning of the test, it publishes the initial position of the ego vehicle and the goal position to set the test
17+
environment. Then, it spawns a sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to
18+
search reacted messages of the planning and control nodes in the pre-determined topics. When all the topics are reacted,
19+
it calculates the reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes
20+
with `spawn_cmd_time`, and it creates a csv file to store the results.
21+
22+
### Perception Planning Mode
23+
24+
In this mode, the reaction analyzer reads the rosbag files which are recorded from AWSIM, and it creates a topic
25+
publisher for each topic inside the rosbag to replay the rosbag. It reads two rosbag files: `path_bag_without_object`
26+
and `path_bag_with_object`. Firstly, it replays the `path_bag_without_object` to set the initial position of the ego
27+
vehicle and the goal position. After `spawn_time_after_init` seconds , it replays the `path_bag_with_object` to spawn a
28+
sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to search the reacted messages of
29+
the perception and planning nodes in the pre-determined topics. When all the topics are reacted, it calculates the
30+
reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes with `spawn_cmd_time`, and
31+
it creates a csv file to store the results.
32+
33+
#### Point Cloud Publisher Type
34+
35+
To get better analyze for Perception & Sensing pipeline, the reaction analyzer can publish the point cloud messages in 3
36+
different ways: `async_header_sync_publish`, `sync_header_sync_publish` or `async_publish`. (`T` is the period of the
37+
lidar's output)
38+
39+
![PointcloudPublisherType.png](media%2FPointcloudPublisherType.png)
40+
41+
- `async_header_sync_publish`: It publishes the point cloud messages synchronously with asynchronous header times. It
42+
means that each of the lidar's output will be published at the same time, but the headers of the point cloud messages
43+
includes different timestamps because of the phase difference.
44+
- `sync_header_sync_publish`: It publishes the point cloud messages synchronously with synchronous header times. It
45+
means that each of the lidar's output will be published at the same time, and the headers of the point cloud messages
46+
includes the same timestamps.
47+
- `async_publish`: It publishes the point cloud messages asynchronously. It means that each of the lidar's output will
48+
be published at different times.
49+
50+
## Usage
51+
52+
The common parameters you need to define for both running modes are `output_file_path`, `test_iteration`,
53+
and `reaction_chain` list. `output_file_path` is the output file path is the path where the results and statistics
54+
will be stored. `test_iteration` defines how many tests will be performed. The `reaction_chain` list is the list of the
55+
pre-defined topics you want to measure their reaction times.
56+
57+
**IMPORTANT:** Ensure the `reaction_chain` list is correctly defined:
58+
59+
- For `perception_planning` mode, **do not** define `Control` nodes.
60+
- For `planning_control` mode, **do not** define `Perception` nodes.
61+
62+
### Prepared Test Environment
63+
64+
- Download the demonstration test map from the
65+
link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After
66+
downloading,
67+
extract the zip file and use its path as `[MAP_PATH]` in the following commands.
68+
69+
#### Planning Control Mode
70+
71+
- You need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters,
72+
you can start to test with the following command:
73+
74+
```bash
75+
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]
76+
```
77+
78+
After the command, the `simple_planning_simulator` and the `reaction_analyzer` will be launched. It will automatically
79+
start to test. After the test is completed, the results will be stored in the `output_file_path` you defined.
80+
81+
#### Perception Planning Mode
82+
83+
- Download the rosbag files from the Google Drive
84+
link [here](https://drive.google.com/file/d/1-Qcv7gYfR-usKOjUH8I997w8I4NMhXlX/view?usp=sharing).
85+
- Extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object`
86+
and `path_bag_with_object`.
87+
- You can start to test with the following command:
88+
89+
```bash
90+
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]
91+
```
92+
93+
After the command, the `e2e_simulator` and the `reaction_analyzer` will be launched. It will automatically start
94+
to test. After the test is completed, the results will be stored in the `output_file_path` you defined.
95+
96+
#### Prepared Test Environment
97+
98+
**Scene without object:**
99+
![sc1-awsim.png](media%2Fsc1-awsim.png)
100+
![sc1-rviz.png](media%2Fsc1-rviz.png)
101+
102+
**Scene object:**
103+
![sc2-awsim.png](media%2Fsc2-awsim.png)
104+
![sc2-rviz.png](media%2Fsc2-rviz.png)
105+
106+
### Custom Test Environment
107+
108+
**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the
109+
parameters.
110+
The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher` (
111+
for `perception_planning` mode) parameters.**
112+
113+
- To set `initialization_pose`, `entity_params`, `goal_pose`:
114+
- Run the AWSIM environment. Tutorial for AWSIM can be found
115+
[here](https://autowarefoundation.github.io/AWSIM/main/GettingStarted/QuickStartDemo/).
116+
- Run the e2e_simulator with the following command:
117+
118+
```bash
119+
ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH]
120+
```
121+
122+
- After EGO is initialized, you can move the ego vehicle to the desired position by using the `SetGoal` button in the
123+
RViz.
124+
- After the EGO stopped in desired position, please localize the dummy obstacle by using the traffic controller. You can
125+
control the traffic by pressing `ESC` button.
126+
127+
**After localize EGO and dummy vehicle, we should write the positions of these entities in the map frame
128+
in `reaction_analyzer.param.yaml`. To achieve this:**
129+
130+
- Get initialization pose from `/awsim/ground_truth/vehicle/pose` topic.
131+
- Get entity params from `/perception/object_recognition/objects` topic.
132+
- Get goal pose from `/planning/mission_planning/goal` topic.
133+
134+
**PS: `initialization_pose` is only valid for `planning_control` mode.**
135+
136+
- After the parameters were noted, we should record the rosbags for the test. To record the rosbags, you can use the
137+
following command:
138+
139+
```bash
140+
ros2 bag record --all
141+
```
142+
143+
- You should record two rosbags: one without the object and one with the object. You can use the traffic controller to
144+
spawn the object in front of the EGO vehicle or remove it.
145+
146+
**NOTE: You should record the rosbags in the same environment with the same position of the EGO vehicle. You don't need
147+
to run Autoware while recording.**
148+
149+
- After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the
150+
paths of the recorded rosbags.
151+
152+
## Results
153+
154+
The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each
155+
pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, `Pipeline Latency`,
156+
and `Total Latency`
157+
for each of the nodes.
158+
159+
- `Node Latency`: The time difference between previous and current node's reaction timestamps. If it is the first node
160+
in the pipeline, it is same as `Pipeline Latency`.
161+
- `Pipeline Latency`: The time difference between published time of the message and pipeline header time.
162+
- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent
163+
timestamp.
164+
165+
## Parameters
166+
167+
| Name | Type | Description |
168+
| ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- |
169+
| `timer_period` | double | [s] Period for the main processing timer. |
170+
| `test_iteration` | int | Number of iterations for the test. |
171+
| `output_file_path` | string | Directory path where test results and statistics will be stored. |
172+
| `spawn_time_after_init` | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode. |
173+
| `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. |
174+
| `poses.initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. |
175+
| `poses.entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
176+
| `poses.goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. |
177+
| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. |
178+
| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. |
179+
| `topic_publisher.spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. |
180+
| `topic_publisher.dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. |
181+
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. |
182+
| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. |
183+
| `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object` | bool | Default false. Publish only the point cloud messages with the object. |
184+
| `reaction_params.first_brake_params.debug_control_commands` | bool | Debug publish flag. |
185+
| `reaction_params.first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. |
186+
| `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. |
187+
| `reaction_params.first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. |
188+
| `reaction_params.search_zero_vel_params.max_looking_distance` | double | [m] Maximum looking distance for zero velocity on trajectory |
189+
| `reaction_params.search_entity_params.search_radius` | double | [m] Searching radius for spawned entity. Distance between ego pose and entity pose. |
190+
| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. |
191+
192+
## Limitations
193+
194+
- Reaction analyzer has some limitation like `PublisherMessageType`, `SubscriberMessageType` and `ReactionType`. It is
195+
currently supporting following list:
196+
197+
- **Publisher Message Types:**
198+
199+
- `sensor_msgs/msg/PointCloud2`
200+
- `sensor_msgs/msg/CameraInfo`
201+
- `sensor_msgs/msg/Image`
202+
- `geometry_msgs/msg/PoseWithCovarianceStamped`
203+
- `sensor_msgs/msg/Imu`
204+
- `autoware_auto_vehicle_msgs/msg/ControlModeReport`
205+
- `autoware_auto_vehicle_msgs/msg/GearReport`
206+
- `autoware_auto_vehicle_msgs/msg/HazardLightsReport`
207+
- `autoware_auto_vehicle_msgs/msg/SteeringReport`
208+
- `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`
209+
- `autoware_auto_vehicle_msgs/msg/VelocityReport`
210+
211+
- **Subscriber Message Types:**
212+
213+
- `sensor_msgs/msg/PointCloud2`
214+
- `autoware_auto_perception_msgs/msg/DetectedObjects`
215+
- `autoware_auto_perception_msgs/msg/TrackedObjects`
216+
- `autoware_auto_msgs/msg/PredictedObject`
217+
- `autoware_auto_planning_msgs/msg/Trajectory`
218+
- `autoware_auto_control_msgs/msg/AckermannControlCommand`
219+
220+
- **Reaction Types:**
221+
- `FIRST_BRAKE`
222+
- `SEARCH_ZERO_VEL`
223+
- `SEARCH_ENTITY`
224+
225+
## Future improvements
226+
227+
- The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`,
228+
`SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of
229+
the message types.

0 commit comments

Comments
 (0)