feat: update stats, check path param, add marker, warn user for wrong…

… reaction_chain

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

tools/reaction_analyzer/README.md

@@ -54,6 +54,11 @@ and `reaction_chain` list. `output_file_path` is the output file path is the pat
 will be stored. `test_iteration` defines how many tests will be performed. The `reaction_chain` list is the list of the
 pre-defined topics you want to measure their reaction times.
 
+**IMPORTANT:** Ensure the `reaction_chain` list is correctly defined:
+
+- For `perception_planning` mode, **do not** define `Control` nodes.
+- For `planning_control` mode, **do not** define `Perception` nodes.
+
 ### Prepared Test Environment
 
 - Download the demonstration test map from the
@@ -119,7 +124,8 @@ ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehic
 - 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.
 
-**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 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:**
 
 - Get initialization pose from `/awsim/ground_truth/vehicle/pose` topic.
 - Get entity params from `/perception/object_recognition/objects` topic.
@@ -146,18 +152,20 @@ to run Autoware while recording.**
 ## 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`, `Pipeline 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.
+- `Node Latency`: The time difference between previous and current node's reaction timestamps. If it is the first node
+  in the pipeline, it is same as `Pipeline Latency`.
 - `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
 
 | 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 statistics will be stored.                                                                              |
@@ -188,31 +196,31 @@ for each of the nodes.
 
 - **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`
+    - `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`
+    - `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`
+    - `FIRST_BRAKE`
+    - `SEARCH_ZERO_VEL`
+    - `SEARCH_ENTITY`
 
 ## Future improvements
 

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -93,6 +93,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   // Publishers
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_initial_pose_;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_goal_pose_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_marker_;
 
   // Variables
   std::vector<PipelineMap> pipeline_map_vector_;
@@ -104,6 +105,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   bool is_initialization_requested{false};
   bool is_route_set_{false};
   size_t test_iteration_count_{0};
+  visualization_msgs::msg::Marker entity_debug_marker_;
 
   // Functions
   void init_analyzer_variables();

tools/reaction_analyzer/include/utils.hpp

@@ -30,6 +30,7 @@
 #include <autoware_internal_msgs/msg/published_time.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
+#include <visualization_msgs/msg/marker.hpp>
 
 #include <boost/uuid/string_generator.hpp>
 #include <boost/uuid/uuid.hpp>
@@ -40,6 +41,7 @@
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <filesystem>
 #include <fstream>
 #include <map>
 #include <string>
@@ -253,6 +255,15 @@ PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityPara
  */
 rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node);
 
+/**
+ * @brief Creates a visualization marker for a polyhedron based on the provided entity parameters.
+ *
+ * @param params The parameters of the entity for which the marker is to be created. It includes the
+ * position, orientation, and dimensions of the entity.
+ * @return The created visualization marker for the polyhedron.
+ */
+visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params);
+
 /**
  * @brief Splits a string by a given delimiter.
  *
@@ -263,6 +274,14 @@ rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node);
  */
 std::vector<std::string> split(const std::string & str, char delimiter);
 
+/**
+ * @brief Checks if a folder exists.
+ *
+ * @param path The path to the folder.
+ * @return True if the folder exists, false otherwise.
+ */
+bool does_folder_exist(const std::string & path);
+
 /**
  * @brief Get the index of the trajectory point that is a certain distance away from the current
  * point.

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -68,9 +68,15 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options)
     return;
   }
 
+  node_params_.output_file_path = get_parameter("output_file_path").as_string();
+  // Check if the output file path is valid
+  if (!does_folder_exist(node_params_.output_file_path)) {
+    RCLCPP_ERROR(get_logger(), "Output file path is not valid. Node couldn't be initialized.");
+    return;
+  }
+
   node_params_.timer_period = get_parameter("timer_period").as_double();
   node_params_.test_iteration = get_parameter("test_iteration").as_int();
-  node_params_.output_file_path = get_parameter("output_file_path").as_string();
   node_params_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double();
   node_params_.spawn_distance_threshold = get_parameter("spawn_distance_threshold").as_double();
 
@@ -115,6 +121,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options)
     create_subscription_options(this));
 
   pub_goal_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("output/goal", rclcpp::QoS(1));
+  pub_marker_ = create_publisher<visualization_msgs::msg::Marker>("~/debug", 10);
 
   init_analyzer_variables();
 
@@ -167,6 +174,8 @@ void ReactionAnalyzerNode::on_timer()
     return;
   }
 
+  pub_marker_->publish(entity_debug_marker_);
+
   // Spawn the obstacle if the conditions are met
   spawn_obstacle(current_odometry_ptr->pose.pose.position);
 
@@ -244,7 +253,7 @@ void ReactionAnalyzerNode::calculate_results(
 void ReactionAnalyzerNode::init_analyzer_variables()
 {
   entity_pose_ = create_entity_pose(node_params_.entity_params);
-
+  entity_debug_marker_ = create_polyhedron_marker(node_params_.entity_params);
   goal_pose_.pose = pose_params_to_pose(node_params_.goal_pose);
 
   if (node_running_mode_ == RunningMode::PlanningControl) {

tools/reaction_analyzer/src/subscriber.cpp

@@ -162,10 +162,14 @@ std::optional<std::map<std::string, MessageBufferVariant>> SubscriberBase::get_m
   for (const auto & [key, variant] : message_buffers_) {
     if (auto * control_message = std::get_if<ControlCommandBuffer>(&variant)) {
       if (!control_message->second) {
+        RCLCPP_WARN_THROTTLE(
+          node_->get_logger(), *node_->get_clock(), 1000, "Waiting for %s to react", key.c_str());
         all_reacted = false;
       }
     } else if (auto * general_message = std::get_if<MessageBuffer>(&variant)) {
       if (!general_message->has_value()) {
+        RCLCPP_WARN_THROTTLE(
+          node_->get_logger(), *node_->get_clock(), 1000, "Waiting for %s to react", key.c_str());
         all_reacted = false;
       }
     }

tools/reaction_analyzer/src/utils.cpp

@@ -120,6 +120,86 @@ rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node)
   return sub_opt;
 }
 
+visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params)
+{
+  visualization_msgs::msg::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time(0);
+  marker.ns = "entity";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+
+  marker.pose.position.x = params.x;
+  marker.pose.position.y = params.y;
+  marker.pose.position.z = params.z;
+
+  tf2::Quaternion quaternion;
+  quaternion.setRPY(
+    tier4_autoware_utils::deg2rad(params.roll), tier4_autoware_utils::deg2rad(params.pitch),
+    tier4_autoware_utils::deg2rad(params.yaw));
+  marker.pose.orientation = tf2::toMsg(quaternion);
+
+  marker.scale.x = 0.1;  // Line width
+
+  marker.color.a = 1.0;  // Alpha
+  marker.color.r = 1.0;
+  marker.color.g = 0.0;
+  marker.color.b = 0.0;
+
+  // Define the 8 corners of the polyhedron
+  geometry_msgs::msg::Point p1, p2, p3, p4, p5, p6, p7, p8;
+
+  p1.x = params.x_l / 2.0;
+  p1.y = params.y_l / 2.0;
+  p1.z = params.z_l / 2.0;
+  p2.x = -params.x_l / 2.0;
+  p2.y = params.y_l / 2.0;
+  p2.z = params.z_l / 2.0;
+  p3.x = -params.x_l / 2.0;
+  p3.y = -params.y_l / 2.0;
+  p3.z = params.z_l / 2.0;
+  p4.x = params.x_l / 2.0;
+  p4.y = -params.y_l / 2.0;
+  p4.z = params.z_l / 2.0;
+  p5.x = params.x_l / 2.0;
+  p5.y = params.y_l / 2.0;
+  p5.z = -params.z_l / 2.0;
+  p6.x = -params.x_l / 2.0;
+  p6.y = params.y_l / 2.0;
+  p6.z = -params.z_l / 2.0;
+  p7.x = -params.x_l / 2.0;
+  p7.y = -params.y_l / 2.0;
+  p7.z = -params.z_l / 2.0;
+  p8.x = params.x_l / 2.0;
+  p8.y = -params.y_l / 2.0;
+  p8.z = -params.z_l / 2.0;
+
+  // Add points to the marker
+  marker.points.push_back(p1);
+  marker.points.push_back(p2);
+  marker.points.push_back(p3);
+  marker.points.push_back(p4);
+  marker.points.push_back(p1);
+
+  marker.points.push_back(p5);
+  marker.points.push_back(p6);
+  marker.points.push_back(p7);
+  marker.points.push_back(p8);
+  marker.points.push_back(p5);
+
+  marker.points.push_back(p1);
+  marker.points.push_back(p5);
+  marker.points.push_back(p6);
+  marker.points.push_back(p2);
+  marker.points.push_back(p3);
+  marker.points.push_back(p7);
+  marker.points.push_back(p4);
+  marker.points.push_back(p8);
+
+  return marker;
+}
+
 std::vector<std::string> split(const std::string & str, char delimiter)
 {
   std::vector<std::string> elements;
@@ -131,6 +211,11 @@ std::vector<std::string> split(const std::string & str, char delimiter)
   return elements;
 }
 
+bool does_folder_exist(const std::string & path)
+{
+  return std::filesystem::exists(path) && std::filesystem::is_directory(path);
+}
+
 size_t get_index_after_distance(
   const Trajectory & traj, const size_t curr_id, const double distance)
 {
@@ -410,7 +495,7 @@ void write_results(
   }
 
   // tmp map to store latency results for statistics
-  std::map<std::string, std::vector<std::pair<double, double>>> tmp_latency_map;
+  std::map<std::string, std::vector<std::tuple<double, double, double>>> tmp_latency_map;
 
   size_t test_count = 0;
   for (const auto & pipeline_map : pipeline_map_vector) {
@@ -444,7 +529,8 @@ void write_results(
           const auto total_latency =
             calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp);
           file << node_latency << " - " << pipeline_latency << " - " << total_latency << ",";
-          tmp_latency_map[node_name].emplace_back(node_latency, total_latency);
+          tmp_latency_map[node_name].emplace_back(
+            std::make_tuple(node_latency, pipeline_latency, total_latency));
         } else {
           const auto & prev_reaction = pipeline_reactions[j - 1].second;
           const auto node_latency =
@@ -454,7 +540,8 @@ void write_results(
           const auto total_latency =
             calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp);
           file << node_latency << " - " << pipeline_latency << " - " << total_latency << ",";
-          tmp_latency_map[node_name].emplace_back(node_latency, total_latency);
+          tmp_latency_map[node_name].emplace_back(
+            std::make_tuple(node_latency, pipeline_latency, total_latency));
         }
       }
       file << "\n";
@@ -465,27 +552,36 @@ void write_results(
 
   file << "\nStatistics\n";
   file << "Node "
-          "Name,Min-NL,Max-NL,Mean-NL,Median-NL,Std-Dev-NL,Min-TL,Max-TL,Mean-TL,Median-TL,Std-Dev-"
-          "TL\n";
+          "Name,Min-NL,Max-NL,Mean-NL,Median-NL,Std-Dev-NL,Min-PL,Max-PL,Mean-PL,Median-PL,Std-Dev-"
+          "PL,Min-TL,Max-TL,Mean-TL,Median-TL,Std-Dev-TL\n";
   for (const auto & [node_name, latency_vec] : tmp_latency_map) {
     file << node_name << ",";
+
     std::vector<double> node_latencies;
+    std::vector<double> pipeline_latencies;
     std::vector<double> total_latencies;
 
     // Extract latencies
     for (const auto & latencies : latency_vec) {
-      node_latencies.push_back(latencies.first);
-      total_latencies.push_back(latencies.second);
+      double node_latency, pipeline_latency, total_latency;
+      std::tie(node_latency, pipeline_latency, total_latency) = latencies;
+      node_latencies.push_back(node_latency);
+      pipeline_latencies.push_back(pipeline_latency);
+      total_latencies.push_back(total_latency);
     }
 
     const auto stats_node_latency = calculate_statistics(node_latencies);
+    const auto stats_pipeline_latency = calculate_statistics(pipeline_latencies);
     const auto stats_total_latency = calculate_statistics(total_latencies);
 
     file << stats_node_latency.min << "," << stats_node_latency.max << ","
          << stats_node_latency.mean << "," << stats_node_latency.median << ","
-         << stats_node_latency.std_dev << "," << stats_total_latency.min << ","
-         << stats_total_latency.max << "," << stats_total_latency.mean << ","
-         << stats_total_latency.median << "," << stats_total_latency.std_dev << "\n";
+         << stats_node_latency.std_dev << "," << stats_pipeline_latency.min << ","
+         << stats_pipeline_latency.max << "," << stats_pipeline_latency.mean << ","
+         << stats_pipeline_latency.median << "," << stats_pipeline_latency.std_dev << ","
+         << stats_total_latency.min << "," << stats_total_latency.max << ","
+         << stats_total_latency.mean << "," << stats_total_latency.median << ","
+         << stats_total_latency.std_dev << "\n";
   }
   file.close();
   RCLCPP_INFO(node->get_logger(), "Results written to: %s", ss.str().c_str());