draft

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

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -121,6 +121,8 @@ struct NodeParams
 {
   std::string running_mode;
   double timer_period;
+  std::string output_file_path;
+  size_t test_iteration;
   double object_search_radius_offset;
   double spawn_time_after_init;
   double spawn_distance_threshold;
@@ -211,6 +213,8 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   void reset();
 
+  void writeResultsToFile();
+
   // Callbacks
   void vehiclePoseCallback(Odometry::ConstSharedPtr msg_ptr);
 
@@ -239,6 +243,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   // Variables
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
   std::unordered_map<std::string, BufferVariant> message_buffers_;
+  std::unordered_map<std::string, std::vector<double>> test_results_;
   std::optional<rclcpp::Time> last_test_environment_init_request_time_;
   std::optional<rclcpp::Time> test_environment_init_time_;
   std::optional<rclcpp::Time> spawn_cmd_time_;
@@ -247,6 +252,8 @@ class ReactionAnalyzerNode : public rclcpp::Node
   bool is_output_printed_{false};
   bool is_vehicle_initialized_{false};
   bool is_route_set_{false};
+  size_t test_iteration_count_{0};
+
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
@@ -258,7 +265,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   // Pointers
   std::shared_ptr<topic_publisher::TopicPublisher> topic_publisher_ptr_;
-
   PointCloud2::SharedPtr entity_pointcloud_ptr_;
   PredictedObjects::SharedPtr predicted_objects_ptr_;
   Odometry::ConstSharedPtr odometry_;

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -1,6 +1,8 @@
 /**:
   ros__parameters:
     timer_period: 0.1 # s
+    test_iteration: 5
+    output_file_path: /home/berkay/projects/reaction_analyzer_results/
     object_search_radius_offset: 0.1 # m
     spawn_time_after_init: 20.0 # s
     spawn_distance_threshold: 25.0 # m
@@ -10,13 +12,20 @@
       control_cmd_buffer_time_interval: 1.0 # s
       min_number_descending_order_control_cmd: 8
       min_jerk_for_brake_cmd: 0.3 # m/s^3
+#    initialization_pose:
+#      x: 1.5
+#      y: 200.0
+#      z: 101.25
+#      roll: 0.0
+#      pitch: 0.0
+#      yaw: 90.0
     initialization_pose:
-      x: 1.5
-      y: 200.0
-      z: 101.25
+      x: 81247.9609375
+      y: 49828.87890625
+      z: 0.0
       roll: 0.0
       pitch: 0.0
-      yaw: 90.0
+      yaw: 35.5
 #    entity_params:
 #      x: 1.5
 #      y: 345.0

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -258,6 +258,8 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
   }
 
   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_.object_search_radius_offset =
     get_parameter("object_search_radius_offset").as_double();
   node_params_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double();
@@ -507,20 +509,12 @@ void ReactionAnalyzerNode::printResults(
       const std::pair<std::string, rclcpp::Time> & a,
       const std::pair<std::string, rclcpp::Time> & b) { return a.second < b.second; });
 
-  // first print spawn_cmd_time to first reaction
-  //  std::cout << "spawn_cmd_time to " << reaction_times.begin()->first << ": "
-  //            << createDurationMs(spawn_cmd_time_.value(), reaction_times.begin()->second)
-  //            << std::endl;
-
-  //  if (reaction_times.size() < 2) return;
-
-  for (size_t i = 0; i < reaction_times.size(); i++) {
-    const auto & first_reaction = reaction_times.at(i);
-    //    const auto & second_reaction = reaction_times.at(i + 1);
-
-    std::cout << "spawn_cmd_time to " << first_reaction.first << ": "
-              << createDurationMs(spawn_cmd_time_.value(), first_reaction.second) << std::endl;
+  for (const auto & reaction_time : reaction_times) {
+    const double duration = createDurationMs(spawn_cmd_time_.value(), reaction_time.second);
+    test_results_[reaction_time.first].emplace_back(duration);
+    std::cout << "spawn_cmd_time to " << reaction_time.first << ": " << duration << std::endl;
   }
+  test_iteration_count_++;
   is_output_printed_ = true;
 }
 
@@ -988,6 +982,11 @@ bool ReactionAnalyzerNode::searchDetectedObjectsNearEntity(const DetectedObjects
 
 void ReactionAnalyzerNode::reset()
 {
+  if (test_iteration_count_ >= node_params_.test_iteration) {
+    writeResultsToFile();
+    RCLCPP_INFO(get_logger(), "Test finished. Node shutting down.");
+    rclcpp::shutdown();
+  }
   is_vehicle_initialized_ = false;
   is_route_set_ = false;
   test_environment_init_time_ = std::nullopt;
@@ -1004,6 +1003,79 @@ void ReactionAnalyzerNode::reset()
   mutex_.unlock();
 }
 
+void ReactionAnalyzerNode::writeResultsToFile()
+{
+  // create csv file
+  auto now = std::chrono::system_clock::now();
+  auto in_time_t = std::chrono::system_clock::to_time_t(now);
+
+  std::stringstream ss;
+  ss << node_params_.output_file_path;
+  if (!node_params_.output_file_path.empty() && node_params_.output_file_path.back() != '/') {
+    ss << "/";  // Ensure the path ends with a slash
+  }
+  if (node_running_mode_ == RunningMode::PlanningControl) {
+    ss << "planning_control-";
+  } else {
+    ss << "perception_planning-";
+  }
+
+  ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d-%H-%M-%S");
+  ss << "-reaction-results.csv";
+
+  // parse the results
+  std::ofstream file(ss.str());
+  // Check if the file was opened successfully
+  if (!file.is_open()) {
+    std::cerr << "Failed to open file: " << ss.str() << std::endl;
+    return;
+  }
+
+  bool is_header_written = false;
+
+  for (const auto & [node, durations] : test_results_) {
+    if (!is_header_written) {
+      file << "Node,";
+      const size_t num_durations = durations.size();
+      for (size_t i = 0; i < num_durations; ++i) {
+        file << "Test - " << i + 1 << ",";
+      }
+      file << "Min,Max,Mean,Median,StdDev\n ";
+      is_header_written = true;
+    }
+
+    // parse test results
+    file << node << ",";
+    for (double i : durations) {
+      file << i << ",";
+    }
+
+    // calculate stats
+    const double min = *std::min_element(durations.begin(), durations.end());
+    const double max = *std::max_element(durations.begin(), durations.end());
+
+    std::vector<double> sorted_data = durations;
+    std::sort(sorted_data.begin(), sorted_data.end());
+    const double median =
+      sorted_data.size() % 2 == 0
+        ? (sorted_data[sorted_data.size() / 2 - 1] + sorted_data[sorted_data.size() / 2]) / 2
+        : sorted_data[sorted_data.size() / 2];
+
+    const double mean =
+      std::accumulate(sorted_data.begin(), sorted_data.end(), 0.0) / sorted_data.size();
+    const double sq_sum = std::inner_product(
+      sorted_data.begin(), sorted_data.end(), sorted_data.begin(), 0.0, std::plus<>(),
+      [&](double a, double b) { return (a - mean) * (b - mean); });
+    double std_dev = std::sqrt(sq_sum / sorted_data.size());
+
+    // parse stats
+    file << min << "," << max << "," << mean << "," << median << "," << std_dev << "\n";
+
+  }
+  file.close();
+  std::cout << "Results written to: " << ss.str() << std::endl;
+}
+
 }  // namespace reaction_analyzer
 
 #include <rclcpp_components/register_node_macro.hpp>