feat: update

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

tools/reaction_analyzer/README.md

@@ -2,10 +2,11 @@
 
 ## Description
 
-The main purpose of the reaction analyzer package is measuring reaction times of the nodes by listening the
-pre-determined topics. It can be used for measuring the reaction time of the perception pipeline, planning pipeline, and
-control pipeline. 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`.
+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)
 
@@ -94,21 +95,26 @@ to test. After the test is completed, the results will be stored in the `output_
 
 ### Custom Test Environment
 
-**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the parameters.
+**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.
+- 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:**
+**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.
+- 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.
+- 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
@@ -117,7 +123,8 @@ 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.**
+**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
 
@@ -131,9 +138,9 @@ vehicle should be same.**
 | `spawn_distance_threshold`                                                   | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode.                                                              |
 | `spawned_pointcloud_sampling_distance`                                       | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode.                                                |
 | `dummy_perception_publisher_period`                                          | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode.                                                      |
-| `initialization_pose`                                                        | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode.                 |
-| `entity_params`                                                              | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. |
-| `goal_pose`                                                                  | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields.                                                        |
+| `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.pointcloud_publisher.pointcloud_publisher_type`             | string | Defines how the PointCloud2 messages are going to be published. Modes explained above.                                                        |

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -72,29 +72,6 @@ enum class RunningMode {
   PlanningControl = 1,
 };
 
-struct PoseParams
-{
-  double x;
-  double y;
-  double z;
-  double roll;
-  double pitch;
-  double yaw;
-};
-
-struct EntityParams
-{
-  double x;
-  double y;
-  double z;
-  double roll;
-  double pitch;
-  double yaw;
-  double x_l;
-  double y_l;
-  double z_l;
-};
-
 struct NodeParams
 {
   std::string running_mode;
@@ -194,7 +171,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
 
   // Pointers
-  std::shared_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
+  std::unique_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
   std::unique_ptr<subscriber::SubscriberBase> subscriber_ptr_;
   PointCloud2::SharedPtr entity_pointcloud_ptr_;
   PredictedObjects::SharedPtr predicted_objects_ptr_;

tools/reaction_analyzer/include/subscriber.hpp

@@ -162,6 +162,10 @@ class SubscriberBase
     rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, geometry_msgs::msg::Pose entity_pose,
     std::atomic<bool> & spawn_object_cmd);
 
+  // Instances of SubscriberBase cannot be copied
+  SubscriberBase(const SubscriberBase &) = delete;
+  SubscriberBase & operator=(const SubscriberBase &) = delete;
+
   ~SubscriberBase() = default;
 
   std::optional<std::unordered_map<std::string, BufferVariant>> getMessageBuffersMap();

tools/reaction_analyzer/include/topic_publisher.hpp

@@ -95,7 +95,7 @@ enum class PointcloudPublisherType {
 template <typename MessageType>
 struct PublisherVariables
 {
-  double period_ns{0.0};
+  std::chrono::milliseconds period_ms{0};
   typename MessageType::SharedPtr empty_area_message;
   typename MessageType::SharedPtr object_spawned_message;
   typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
@@ -150,15 +150,15 @@ struct PublisherVarAccessor
   }
 
   template <typename T>
-  void set_period(T & publisherVar, double newPeriod)
+  void set_period(T & publisherVar, std::chrono::milliseconds new_period)
   {
-    publisherVar.period_ns = newPeriod;
+    publisherVar.period_ms = new_period;
   }
 
   template <typename T>
-  double get_period(const T & publisherVar) const
+  std::chrono::milliseconds get_period(const T & publisherVar) const
   {
-    return publisherVar.period_ns;
+    return publisherVar.period_ms;
   }
 
   template <typename T>
@@ -207,7 +207,8 @@ class TopicPublisher
   // Initialized variables
   rclcpp::Node * node_;
   std::atomic<bool> & spawn_object_cmd_;
-  std::optional<rclcpp::Time> & spawn_cmd_time_;
+  std::optional<rclcpp::Time> & spawn_cmd_time_;  // Set by a publisher function when the
+                                                  // spawn_object_cmd_ is true
 
   // Parameters
   TopicPublisherParams topic_publisher_params_;

tools/reaction_analyzer/include/utils.hpp

@@ -31,6 +31,32 @@ namespace reaction_analyzer
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 
+/**
+ * @brief A struct containing the parameters of a pose.
+ */
+struct PoseParams
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+};
+
+struct EntityParams
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double x_l;
+  double y_l;
+  double z_l;
+};
+
 /**
  * @brief Sorts the test results by their median value.
  *

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -8,30 +8,31 @@
     spawn_distance_threshold: 15.0 # m # for planning_control mode
     spawned_pointcloud_sampling_distance: 0.1
     dummy_perception_publisher_period: 0.1 # s
-    initialization_pose:
-      x: 81247.9609375
-      y: 49828.87890625
-      z: 0.0
-      roll: 0.0
-      pitch: 0.0
-      yaw: 35.5
-    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
-    goal_pose:
-      x: 81824.90625
-      y: 50069.34375
-      z: 0.0
-      roll: 0.0
-      pitch: 0.0
-      yaw: 8.9305903
+    poses:
+      initialization_pose:
+        x: 81247.9609375
+        y: 49828.87890625
+        z: 0.0
+        roll: 0.0
+        pitch: 0.0
+        yaw: 35.5
+      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
+      goal_pose:
+        x: 81824.90625
+        y: 50069.34375
+        z: 0.0
+        roll: 0.0
+        pitch: 0.0
+        yaw: 8.9305903
     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

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -80,29 +80,29 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
     get_parameter("dummy_perception_publisher_period").as_double();
 
   // Position parameters
-  node_params_.initial_pose.x = get_parameter("initialization_pose.x").as_double();
-  node_params_.initial_pose.y = get_parameter("initialization_pose.y").as_double();
-  node_params_.initial_pose.z = get_parameter("initialization_pose.z").as_double();
-  node_params_.initial_pose.roll = get_parameter("initialization_pose.roll").as_double();
-  node_params_.initial_pose.pitch = get_parameter("initialization_pose.pitch").as_double();
-  node_params_.initial_pose.yaw = get_parameter("initialization_pose.yaw").as_double();
-
-  node_params_.goal_pose.x = get_parameter("goal_pose.x").as_double();
-  node_params_.goal_pose.y = get_parameter("goal_pose.y").as_double();
-  node_params_.goal_pose.z = get_parameter("goal_pose.z").as_double();
-  node_params_.goal_pose.roll = get_parameter("goal_pose.roll").as_double();
-  node_params_.goal_pose.pitch = get_parameter("goal_pose.pitch").as_double();
-  node_params_.goal_pose.yaw = get_parameter("goal_pose.yaw").as_double();
-
-  node_params_.entity_params.x = get_parameter("entity_params.x").as_double();
-  node_params_.entity_params.y = get_parameter("entity_params.y").as_double();
-  node_params_.entity_params.z = get_parameter("entity_params.z").as_double();
-  node_params_.entity_params.roll = get_parameter("entity_params.roll").as_double();
-  node_params_.entity_params.pitch = get_parameter("entity_params.pitch").as_double();
-  node_params_.entity_params.yaw = get_parameter("entity_params.yaw").as_double();
-  node_params_.entity_params.x_l = get_parameter("entity_params.x_dimension").as_double();
-  node_params_.entity_params.y_l = get_parameter("entity_params.y_dimension").as_double();
-  node_params_.entity_params.z_l = get_parameter("entity_params.z_dimension").as_double();
+  node_params_.initial_pose.x = get_parameter("poses.initialization_pose.x").as_double();
+  node_params_.initial_pose.y = get_parameter("poses.initialization_pose.y").as_double();
+  node_params_.initial_pose.z = get_parameter("poses.initialization_pose.z").as_double();
+  node_params_.initial_pose.roll = get_parameter("poses.initialization_pose.roll").as_double();
+  node_params_.initial_pose.pitch = get_parameter("poses.initialization_pose.pitch").as_double();
+  node_params_.initial_pose.yaw = get_parameter("poses.initialization_pose.yaw").as_double();
+
+  node_params_.goal_pose.x = get_parameter("poses.goal_pose.x").as_double();
+  node_params_.goal_pose.y = get_parameter("poses.goal_pose.y").as_double();
+  node_params_.goal_pose.z = get_parameter("poses.goal_pose.z").as_double();
+  node_params_.goal_pose.roll = get_parameter("poses.goal_pose.roll").as_double();
+  node_params_.goal_pose.pitch = get_parameter("poses.goal_pose.pitch").as_double();
+  node_params_.goal_pose.yaw = get_parameter("poses.goal_pose.yaw").as_double();
+
+  node_params_.entity_params.x = get_parameter("poses.entity_params.x").as_double();
+  node_params_.entity_params.y = get_parameter("poses.entity_params.y").as_double();
+  node_params_.entity_params.z = get_parameter("poses.entity_params.z").as_double();
+  node_params_.entity_params.roll = get_parameter("poses.entity_params.roll").as_double();
+  node_params_.entity_params.pitch = get_parameter("poses.entity_params.pitch").as_double();
+  node_params_.entity_params.yaw = get_parameter("poses.entity_params.yaw").as_double();
+  node_params_.entity_params.x_l = get_parameter("poses.entity_params.x_dimension").as_double();
+  node_params_.entity_params.y_l = get_parameter("poses.entity_params.y_dimension").as_double();
+  node_params_.entity_params.z_l = get_parameter("poses.entity_params.z_dimension").as_double();
 
   sub_kinematics_ = create_subscription<Odometry>(
     "input/kinematics", 1, std::bind(&ReactionAnalyzerNode::on_vehicle_pose, this, _1),
@@ -143,7 +143,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
   } else if (node_running_mode_ == RunningMode::PerceptionPlanning) {
     // Create topic publishers
     topic_publisher_ptr_ =
-      std::make_shared<topic_publisher::TopicPublisher>(this, spawn_object_cmd_, spawn_cmd_time_);
+      std::make_unique<topic_publisher::TopicPublisher>(this, spawn_object_cmd_, spawn_cmd_time_);
 
     // Subscribe to the ground truth position
     sub_ground_truth_pose_ = create_subscription<PoseStamped>(
@@ -183,15 +183,19 @@ void ReactionAnalyzerNode::on_timer()
     return;
   }
 
+  // Spawn the obstacle if the conditions are met
   spawn_obstacle(current_odometry_ptr->pose.pose.position);
 
+  // If the spawn_cmd_time is not set by pointcloud publishers, don't do anything
   if (!spawn_cmd_time) return;
 
+  // Get the reacted messages, if all of the modules reacted
   const auto message_buffers = subscriber_ptr_->getMessageBuffersMap();
 
   if (message_buffers) {
     // if reacted, calculate the results
     calculate_results(message_buffers.value(), spawn_cmd_time.value());
+    // reset the variables if the iteration is not finished, otherwise write the results
     reset();
   }
 }

tools/reaction_analyzer/src/topic_publisher.cpp

@@ -771,14 +771,15 @@ void TopicPublisher::set_period_to_variable_map(
         total_time_diff_ns += (timestamps_tmp[i] - timestamps_tmp[i - 1]).nanoseconds();
       }
 
-      // Convert to double for the division to get the average period in nanoseconds
-      double period_ns =
-        static_cast<double>(total_time_diff_ns) / static_cast<double>(timestamps_tmp.size() - 1);
+      // Conversion to std::chrono::milliseconds
+      auto total_duration_ns = std::chrono::nanoseconds(total_time_diff_ns);
+      auto period_ms = std::chrono::duration_cast<std::chrono::milliseconds>(total_duration_ns) /
+                       (timestamps_tmp.size() - 1);
 
       PublisherVariablesVariant & publisherVar = topic_publisher_map_[topic_name];
       PublisherVarAccessor accessor;
 
-      std::visit([&](auto & var) { accessor.set_period(var, period_ns); }, publisherVar);
+      std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisherVar);
     }
   }
 }

tools/reaction_analyzer/src/utils.cpp

@@ -35,14 +35,9 @@ std::vector<std::tuple<std::string, std::vector<double>, double>> sort_results_b
     auto vec = pair.second;
 
     // Calculate the median
-    std::sort(vec.begin(), vec.end());
-    double median = 0.0;
-    size_t size = vec.size();
-    if (size % 2 == 0) {
-      median = (vec[size / 2 - 1] + vec[size / 2]) / 2.0;
-    } else {
-      median = vec[size / 2];
-    }
+    const size_t mid_index = vec.size() / 2;
+    std::nth_element(vec.begin(), vec.begin() + mid_index, vec.end());
+    const double median = vec[mid_index];
 
     sorted_data.emplace_back(pair.first, pair.second, median);
   }