Skip to content

Commit

Permalink
feat: update style and readme
Browse files Browse the repository at this point in the history
Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 committed Mar 5, 2024
1 parent 3c480f1 commit 94ca733
Show file tree
Hide file tree
Showing 6 changed files with 165 additions and 119 deletions.
117 changes: 83 additions & 34 deletions tools/reaction_analyzer/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -55,68 +55,77 @@ pre-defined topics you want to measure their reaction times.

### Prepared Test Environment

Firstly, you need to 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.
- 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

Firstly, 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:
- 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.
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

Firstly, you need to download the rosbag files from the
link [here](https://drive.google.com/file/d/1_P-3oy_M6eJ7fk8h5CP8V6s6da6pPrBu/view?usp=sharing). After downloading,
extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object`
and `path_bag_with_object`. Because custom sensor setup, you need to checkout the following branches before launch the
reaction analyzer: For the `autoware_individual_params` repository, checkout the
branch [here](https://github.com/brkay54/autoware_individual_params/tree/bk/reaction-analyzer-config). For
the `awsim_sensor_kit_launch` repository, checkout the
branch [here](https://github.com/brkay54/awsim_sensor_kit_launch/tree/bk/reaction-analyzer-config).
After you checkouted the branches, you can start to test with the following command:
- Download the rosbag files from the Google Drive
link [here](https://drive.google.com/file/d/1_P-3oy_M6eJ7fk8h5CP8V6s6da6pPrBu/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`.
- Because custom sensor setup, you need to check out the following branches before launch the
reaction analyzer: For the `autoware_individual_params` repository, check out the
branch [here](https://github.com/brkay54/autoware_individual_params/tree/bk/reaction-analyzer-config).
- For the `awsim_sensor_kit_launch` repository, check out the
branch [here](https://github.com/brkay54/awsim_sensor_kit_launch/tree/bk/reaction-analyzer-config).
- After you check outed the branches, 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_sensor_kit map_path:=[MAP_PATH]
```

After the command, the e2e_simulator and the reaction_analyzer will be launched. It will automatically start
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.

### 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`
parameters.
For `initialization_pose`, `entity_params`, `goal_pose`, you can upload your `.osm` map file into
the [scenario editor](https://scenario.ci.tier4.jp/scenario_editor/) to define the position of the position parameters.
You can define the positions with respect to you desired root. Firstly, add EGO vehicle from edit/add entity/Ego to map.
Set destination to EGO vehicle and add another dummy object in same way. The dummy object represents the object spawn
suddenly in the reaction analyzer test. After you set up the positions in the map, we should get the positions of these
entities in the map frame. To achieve this, you should convert the positions to map frame by changing Map/Coordinate to
World and Map/Orientation to Euler. After these steps, you can see the positions in map frame and euler angles. You can
change the `initialization_pose`, `entity_params`, `goal_pose` parameters with the values you get from the website.

For the `topic_publisher` parameters, you need to record the rosbags from the AWSIM. After opened your AWSIM
**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`:
- Upload your `.osm` map file into the [scenario editor](https://scenario.ci.tier4.jp/scenario_editor/) to define the position of the position parameters.
- Add EGO vehicle from edit/add entity/Ego to map.
- Set destination to EGO vehicle and add another dummy object in same way. The dummy object represents the object spawn
suddenly in the reaction analyzer test.

**After you set up the positions in the map, we should get the positions of these entities in the map frame. To achieve this:**

- Convert the positions to map frame by changing Map/Coordinate to World and Map/Orientation to Euler in Scenario Editor.

- After these steps, you can see the positions in map frame and euler angles. You can change the `initialization_pose`, `entity_params`, `goal_pose` parameters with the values you get from the website.

**For the `topic_publisher` parameters, you need to record the rosbags from the AWSIM. After opened your AWSIM
environment, you should record two different rosbags. However, the environment should be static and the position of the
vehicle should be same. Firstly, record a rosbag in empty environment. After that, record another rosbag in the same
environment except add an object in front of the vehicle. 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.
vehicle should be same.**

- Record a rosbag in empty environment (without an obstacle in front of the vehicle).
- After that, record another rosbag in the same environment except add an object in front of the vehicle.

**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.**

## 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 statictics will be stored. |
| `output_file_path` | string | Directory path where test results and statistics will be stored. |
| `object_search_radius_offset` | double | [m] Additional radius added to the search area when looking for objects. |
| `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. |
Expand All @@ -136,3 +145,43 @@ the `path_bag_without_object` and `path_bag_with_object` parameters with the pat
| `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.
30 changes: 15 additions & 15 deletions tools/reaction_analyzer/include/topic_publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,18 +54,18 @@ using nav_msgs::msg::Odometry;
using sensor_msgs::msg::PointCloud2;

enum class PublisherMessageType {
Unknown = 0,
CameraInfo = 1,
Image = 2,
PointCloud2 = 3,
PoseWithCovarianceStamped = 4,
Imu = 5,
ControlModeReport = 6,
GearReport = 7,
HazardLightsReport = 8,
SteeringReport = 9,
TurnIndicatorsReport = 10,
VelocityReport = 11,
UNKNOWN = 0,
CAMERA_INFO = 1,
IMAGE = 2,
POINTCLOUD2 = 3,
POSE_WITH_COVARIANCE_STAMPED = 4,
IMU = 5,
CONTROL_MODE_REPORT = 6,
GEAR_REPORT = 7,
HAZARD_LIGHTS_REPORT = 8,
STEERING_REPORT = 9,
TURN_INDICATORS_REPORT = 10,
VELOCITY_REPORT = 11,
};

struct TopicPublisherParams
Expand All @@ -77,9 +77,9 @@ struct TopicPublisherParams
};

enum class PointcloudPublisherType {
AsyncPublisher = 0, // Asynchronous publisher
SyncHeaderSyncPublisher = 1, // Synchronous publisher with header synchronization
AsyncHeaderSyncPublisher = 2, // Asynchronous publisher with header synchronization
ASYNC_PUBLISHER = 0, // Asynchronous publisher
SYNC_HEADER_SYNC_PUBLISHER = 1, // Synchronous publisher with header synchronization
ASYNC_HEADER_SYNC_PUBLISHER = 2, // Asynchronous publisher with header synchronization
};

/**
Expand Down
32 changes: 16 additions & 16 deletions tools/reaction_analyzer/param/reaction_analyzer.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -53,64 +53,64 @@
obstacle_stop_planner:
topic_name: /planning/scenario_planning/lane_driving/trajectory
time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
scenario_selector:
topic_name: /planning/scenario_planning/scenario_selector/trajectory
time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
motion_velocity_smoother:
topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory
time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
planning_validator:
topic_name: /planning/scenario_planning/trajectory
time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time
message_type: autoware_auto_planning_msgs::msg::Trajectory
message_type: autoware_auto_planning_msgs/msg/Trajectory
# trajectory_follower:
# topic_name: /control/trajectory_follower/control_cmd
# time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time
# message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
# message_type: autoware_auto_control_msgs/msg/AckermannControlCommand
# vehicle_cmd_gate:
# topic_name: /control/command/control_cmd
# time_debug_topic_name: /control/command/control_cmd/debug/published_time
# message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
# message_type: autoware_auto_control_msgs/msg/AckermannControlCommand
common_ground_filter:
topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw
time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw/debug/published_time
message_type: sensor_msgs::msg::PointCloud2
message_type: sensor_msgs/msg/PointCloud2
occupancy_grid_map_outlier:
topic_name: /perception/obstacle_segmentation/pointcloud
time_debug_topic_name: /perception/obstacle_segmentation/pointcloud/debug/published_time
message_type: sensor_msgs::msg::PointCloud2
message_type: sensor_msgs/msg/PointCloud2
multi_object_tracker:
topic_name: /perception/object_recognition/tracking/near_objects
time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::TrackedObjects
message_type: autoware_auto_perception_msgs/msg/TrackedObjects
lidar_centerpoint:
topic_name: /perception/object_recognition/detection/centerpoint/objects
time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
obstacle_pointcloud_based_validator:
topic_name: /perception/object_recognition/detection/centerpoint/validation/objects
time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
decorative_tracker_merger:
topic_name: /perception/object_recognition/tracking/objects
time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::TrackedObjects
message_type: autoware_auto_perception_msgs/msg/TrackedObjects
detected_object_feature_remover:
topic_name: /perception/object_recognition/detection/clustering/objects
time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
detection_by_tracker:
topic_name: /perception/object_recognition/detection/detection_by_tracker/objects
time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
object_lanelet_filter:
topic_name: /perception/object_recognition/detection/objects
time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::DetectedObjects
message_type: autoware_auto_perception_msgs/msg/DetectedObjects
map_based_prediction:
topic_name: /perception/object_recognition/objects
time_debug_topic_name: /perception/object_recognition/objects/debug/published_time
message_type: autoware_auto_perception_msgs::msg::PredictedObjects
message_type: autoware_auto_perception_msgs/msg/PredictedObjects
2 changes: 0 additions & 2 deletions tools/reaction_analyzer/src/reaction_analyzer_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,6 @@ void ReactionAnalyzerNode::onTimer()
mutex_.unlock();

// Init the test environment

if (!test_environment_init_time_) {
initEgoForTest(initialization_state_ptr, route_state_ptr, operation_mode_ptr);
return;
Expand All @@ -176,7 +175,6 @@ void ReactionAnalyzerNode::onTimer()

if (message_buffers) {
// if reacted, calculate the results

calculateResults(message_buffers.value(), spawn_cmd_time.value());
reset();
}
Expand Down
Loading

0 comments on commit 94ca733

Please sign in to comment.