Skip to content

Commit

Permalink
feat: update for new sensor setup, fix bug, optimize code, show pipel…
Browse files Browse the repository at this point in the history
…ine latency, update readme

Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 authored and Berkay Karaman committed May 17, 2024
1 parent 4af034c commit d459e4a
Show file tree
Hide file tree
Showing 12 changed files with 109 additions and 170 deletions.
75 changes: 47 additions & 28 deletions tools/reaction_analyzer/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -76,23 +76,28 @@ start to test. After the test is completed, the results will be stored in the `o
#### Perception Planning Mode

- Download the rosbag files from the Google Drive
link [here](https://drive.google.com/file/d/1_P-3oy_M6eJ7fk8h5CP8V6s6da6pPrBu/view?usp=sharing).
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`.
- 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:
- 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]
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
Expand All @@ -101,39 +106,53 @@ The parameters you need to redefine are `initialization_pose`, `entity_params`,
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.
- 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 you set up the positions in the map, we should get the positions of these entities in the map frame. To achieve
this:**
- 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.

- Convert the positions to map frame by changing Map/Coordinate to World and Map/Orientation to Euler in Scenario
Editor.
**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:**

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

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

- 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.
**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.**
- 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`, and `Total Latency`
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.
- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent timestamp.
- `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

Expand Down
2 changes: 1 addition & 1 deletion tools/reaction_analyzer/include/reaction_analyzer_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
std::optional<rclcpp::Time> test_environment_init_time_;
std::optional<rclcpp::Time> spawn_cmd_time_;
std::atomic<bool> spawn_object_cmd_{false};
bool is_vehicle_initialized_{false};
bool is_initialization_requested{false};
bool is_route_set_{false};
size_t test_iteration_count_{0};

Expand Down
7 changes: 3 additions & 4 deletions tools/reaction_analyzer/include/topic_publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,13 +223,12 @@ class TopicPublisher
const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message,
const bool is_empty_area_message);
void set_period(const std::map<std::string, std::vector<rclcpp::Time>> & time_map);
std::map<std::string, PublisherVariables<PointCloud2>> set_general_publishers_and_timers();
void set_pointcloud_publishers_and_timers(
void set_publishers_and_timers_to_variable();
void set_timers_for_pointcloud_msgs(
const std::map<std::string, PublisherVariables<PointCloud2>> & pointcloud_variables_map);
bool check_publishers_initialized_correctly();
void init_rosbag_publishers();
void init_rosbag_publishers_with_object(const std::string & path_bag_with_object);
void init_rosbag_publishers_without_object(const std::string & path_bag_without_object);
void init_rosbag_publisher_buffer(const std::string & bag_path, const bool is_empty_area_message);
void pointcloud_messages_sync_publisher(const PointcloudPublisherType type);
void pointcloud_messages_async_publisher(
const std::pair<
Expand Down
5 changes: 2 additions & 3 deletions tools/reaction_analyzer/launch/reaction_analyzer.launch.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<launch>
<arg name="reaction_analyzer_param_path" default="$(find-pkg-share reaction_analyzer)/param/reaction_analyzer.param.yaml"/>

<!-- reaction analyzer publishes own perception objects -->
<!-- reaction analyzer publishes own perception objects for planning_control mode -->
<arg name="launch_simulator_perception_modules" default="false"/>

<!-- Arguments for occupancy_grid_map -->
Expand All @@ -23,7 +23,6 @@
<arg name="vehicle_model" value="$(var vehicle_model)"/>
<arg name="sensor_model" value="$(var sensor_model)"/>
<arg name="use_sim_time" value="false"/>
<arg name="enable_image_decompressor" value="false"/>
</include>
</group>
<group if="$(eval &quot;'$(var running_mode)'=='planning_control'&quot;)">
Expand Down Expand Up @@ -51,6 +50,7 @@
<node_container pkg="rclcpp_components" exec="component_container_mt" name="reaction_analyzer_container" namespace="" args="" output="screen">
<composable_node pkg="reaction_analyzer" plugin="reaction_analyzer::ReactionAnalyzerNode" name="reaction_analyzer" namespace="">
<param from="$(var reaction_analyzer_param_path)"/>
<param name="running_mode" value="$(var running_mode)"/>
<remap from="output/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
<remap from="output/objects" to="/perception/object_recognition/objects"/>
<remap from="output/initialpose" to="/initialpose"/>
Expand All @@ -61,7 +61,6 @@
<remap from="input/routing_state" to="/api/routing/state"/>
<remap from="input/operation_mode_state" to="/api/operation_mode/state"/>
<remap from="input/ground_truth_pose" to="/awsim/ground_truth/vehicle/pose"/>
<param name="running_mode" value="$(var running_mode)"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>
</node_container>
Expand Down
Binary file added tools/reaction_analyzer/media/sc1-awsim.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added tools/reaction_analyzer/media/sc1-rviz.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added tools/reaction_analyzer/media/sc2-awsim.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added tools/reaction_analyzer/media/sc2-rviz.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
36 changes: 18 additions & 18 deletions tools/reaction_analyzer/param/reaction_analyzer.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2,37 +2,37 @@
ros__parameters:
timer_period: 0.033 # s
test_iteration: 10
output_file_path: <PATH_TO_RESUlTS_FOLDER>
output_file_path: <PATH_TO_OUTPUT_FOLDER>
spawn_time_after_init: 10.0 # s for perception_planning mode
spawn_distance_threshold: 15.0 # m # for planning_control mode
poses:
initialization_pose:
x: 81247.9609375
y: 49828.87890625
x: 81546.984375
y: 50011.96875
z: 0.0
roll: 0.0
pitch: 0.0
yaw: 35.5
yaw: 11.1130405
goal_pose:
x: 81824.90625
y: 50069.34375
x: 81643.0703125
y: 50029.8828125
z: 0.0
roll: 0.0
pitch: 0.0
yaw: 8.9305903
yaw: 13.12
entity_params:
x: 81392.97671487389
y: 49927.361443601316
z: 42.13605499267578
roll: 0.2731273
pitch: -0.6873504
yaw: 33.7664809
x_dimension: 4.118675972722859
y_dimension: 1.7809072588403219
z_dimension: 0.8328610206872963
x: 81633.86068376624
y: 50028.383586673124
z: 42.44818343779461
roll: 0.0
pitch: 0.0
yaw: 11.8235848
x_dimension: 3.95
y_dimension: 1.77
z_dimension: 1.43
topic_publisher:
path_bag_without_object: <PATH_TO_YOUR_BAGS>/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
path_bag_with_object: <PATH_TO_YOUR_BAGS>/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3
path_bag_without_object: <PATH_TO_YOUR_BAGS>/rosbag2_awsim_labs/rosbag2_awsim_labs.db3
path_bag_with_object: <PATH_TO_YOUR_BAGS>/rosbag2_awsim_labs_obstacle/rosbag2_awsim_labs_obstacle.db3
pointcloud_publisher:
pointcloud_publisher_type: "sync_header_sync_publish" # "async_header_sync_publish", "sync_header_sync_publish" or "async_publish"
pointcloud_publisher_period: 0.1 # s
Expand Down
29 changes: 15 additions & 14 deletions tools/reaction_analyzer/src/reaction_analyzer_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -269,11 +269,14 @@ void ReactionAnalyzerNode::init_test_env(
last_test_environment_init_request_time_ = current_time;

// Pose initialization of the ego
is_vehicle_initialized_ = !is_vehicle_initialized_
? init_ego(initialization_state_ptr, odometry_ptr, current_time)
: is_vehicle_initialized_;
is_initialization_requested = !is_initialization_requested
? init_ego(initialization_state_ptr, odometry_ptr, current_time)
: is_initialization_requested;

if (!is_vehicle_initialized_) {
if (
is_initialization_requested &&
initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) {
is_initialization_requested = false;
return;
}

Expand All @@ -298,7 +301,7 @@ void ReactionAnalyzerNode::init_test_env(
}

const bool is_ready =
(is_vehicle_initialized_ && is_route_set_ &&
(is_initialization_requested && is_route_set_ &&
(operation_mode_ptr->mode == OperationModeState::AUTONOMOUS ||
node_running_mode_ == RunningMode::PerceptionPlanning));
if (is_ready) {
Expand Down Expand Up @@ -332,14 +335,12 @@ bool ReactionAnalyzerNode::init_ego(
{
// Pose initialization of the ego
if (initialization_state_ptr) {
if (initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) {
if (node_running_mode_ == RunningMode::PlanningControl) {
// publish initial pose
init_pose_.header.stamp = current_time;
init_pose_.header.frame_id = "map";
pub_initial_pose_->publish(init_pose_);
}
return false;
if (node_running_mode_ == RunningMode::PlanningControl) {
// publish initial pose
init_pose_.header.stamp = current_time;
init_pose_.header.frame_id = "map";
pub_initial_pose_->publish(init_pose_);
RCLCPP_WARN_ONCE(get_logger(), "Initialization position is published. Waiting for init...");
}
// Wait until odometry_ptr is initialized
if (!odometry_ptr) {
Expand Down Expand Up @@ -407,7 +408,7 @@ void ReactionAnalyzerNode::reset()
return;
}
// reset the variables
is_vehicle_initialized_ = false;
is_initialization_requested = false;
is_route_set_ = false;
test_environment_init_time_ = std::nullopt;
last_test_environment_init_request_time_ = std::nullopt;
Expand Down
Loading

0 comments on commit d459e4a

Please sign in to comment.