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

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>

Author: brkay54

tools/reaction_analyzer/README.md

@@ -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
@@ -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
 

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -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};
 

tools/reaction_analyzer/include/topic_publisher.hpp

@@ -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<

tools/reaction_analyzer/launch/reaction_analyzer.launch.xml

@@ -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 -->
@@ -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;)">
@@ -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"/>
@@ -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>

tools/reaction_analyzer/media/sc1-awsim.png

@@ -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

tools/reaction_analyzer/media/sc1-rviz.png

@@ -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;
     }
 
@@ -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) {
@@ -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) {
@@ -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;