feat: improve style, change csv output stringstream

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

Author: brkay54

tools/reaction_analyzer/README.md

@@ -126,23 +126,31 @@ vehicle should be same.**
 **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`
+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.
+
 ## 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.                                                                              |
-| `object_search_radius_offset`                                                | double | [m] Additional radius added to the search area when looking for objects.                                                                      |
 | `spawn_time_after_init`                                                      | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode.                                           |
 | `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.                                                      |
 | `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.spawned_pointcloud_sampling_distance`                       | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode.                                                |
+| `topic_publisher.dummy_perception_publisher_period`                          | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode.                                                      |
 | `topic_publisher.pointcloud_publisher.pointcloud_publisher_type`             | string | Defines how the PointCloud2 messages are going to be published. Modes explained above.                                                        |
 | `topic_publisher.pointcloud_publisher.pointcloud_publisher_period`           | double | [s] Publishing period of the PointCloud2 messages.                                                                                            |
 | `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object`   | bool   | Default false. Publish only the point cloud messages with the object.                                                                         |
@@ -191,5 +199,4 @@ paths of the recorded rosbags.**
 
 - The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`,
   `SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of
-  the
-  message types.
+  the message types.

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -15,15 +15,8 @@
 #ifndef REACTION_ANALYZER_NODE_HPP_
 #define REACTION_ANALYZER_NODE_HPP_
 
-#include "tf2/transform_datatypes.h"
-
-#include <pcl/impl/point_types.hpp>
-#include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <subscriber.hpp>
-#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
-#include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <topic_publisher.hpp>
 #include <utils.hpp>
 
@@ -34,17 +27,7 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
-#include <boost/uuid/string_generator.hpp>
-#include <boost/uuid/uuid.hpp>
-#include <boost/uuid/uuid_generators.hpp>
-#include <boost/uuid/uuid_io.hpp>
-
-#include <pcl/common/distances.h>
-#include <pcl/point_types.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-
+#include <algorithm>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -61,17 +44,12 @@ using autoware_adapi_v1_msgs::msg::RouteState;
 using autoware_adapi_v1_msgs::srv::ChangeOperationMode;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_internal_msgs::msg::PublishedTime;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseStamped;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
 
-// The running mode of the node
-enum class RunningMode {
-  PerceptionPlanning = 0,
-  PlanningControl = 1,
-};
-
 struct NodeParams
 {
   std::string running_mode;
@@ -80,8 +58,6 @@ struct NodeParams
   size_t test_iteration;
   double spawn_time_after_init;
   double spawn_distance_threshold;
-  double spawned_pointcloud_sampling_distance;
-  double dummy_perception_publisher_period;
   PoseParams initial_pose;
   PoseParams goal_pose;
   EntityParams entity_params;
@@ -112,33 +88,25 @@ class ReactionAnalyzerNode : public rclcpp::Node
   rclcpp::Subscription<RouteState>::SharedPtr sub_route_state_;
   rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_init_state_;
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
-  rclcpp::Subscription<PoseStamped>::SharedPtr sub_ground_truth_pose_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr
+    sub_ground_truth_pose_;  // Only for perception_planning mode
 
   // Publishers
-  rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_;
-  rclcpp::Publisher<PredictedObjects>::SharedPtr pub_predicted_objects_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_initial_pose_;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_goal_pose_;
 
-  // tf
-  tf2_ros::Buffer tf_buffer_{get_clock()};
-  tf2_ros::TransformListener tf_listener_{tf_buffer_};
-
   // Variables
-  std::unordered_map<std::string, std::vector<double>> test_results_;
+  std::vector<PipelineMap> pipeline_map_vector_;
   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_;
   std::atomic<bool> spawn_object_cmd_{false};
-  std::atomic<bool> is_object_spawned_message_published_{false};
   bool is_vehicle_initialized_{false};
   bool is_route_set_{false};
   size_t test_iteration_count_{0};
 
   // Functions
   void init_analyzer_variables();
-  void init_pointcloud();
-  void init_predicted_objects();
   void init_ego(
     const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
     const RouteState::ConstSharedPtr & route_state_ptr,
@@ -148,12 +116,10 @@ class ReactionAnalyzerNode : public rclcpp::Node
   void call_operation_mode_service_without_response();
   void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose);
   void calculate_results(
-    const std::unordered_map<std::string, subscriber::BufferVariant> & message_buffers,
+    const std::unordered_map<std::string, subscriber::MessageBufferVariant> & message_buffers,
     const rclcpp::Time & spawn_cmd_time);
   void on_timer();
-  void dummy_perception_publisher();
   void reset();
-  void write_results();
 
   // Callbacks
   void on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr);
@@ -165,16 +131,13 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;
-  rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
 
   // Client
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
 
   // Pointers
   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_;
   LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_;
   RouteState::ConstSharedPtr current_route_state_ptr_;
   OperationModeState::ConstSharedPtr operation_mode_ptr_;

tools/reaction_analyzer/include/subscriber.hpp

@@ -15,31 +15,17 @@
 #ifndef SUBSCRIBER_HPP_
 #define SUBSCRIBER_HPP_
 #include <motion_utils/trajectory/trajectory.hpp>
-#include <pcl/impl/point_types.hpp>
-#include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
-#include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <utils.hpp>
 
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
-#include <autoware_internal_msgs/msg/published_time.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <message_filters/cache.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/exact_time.h>
 #include <message_filters/synchronizer.h>
-#include <pcl/common/distances.h>
-#include <pcl/point_types.h>
-#include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -66,19 +52,12 @@ using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
 
-// Buffers to be used to store subscribed messages
-using ControlCommandBuffer =
-  std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
-using TrajectoryBuffer = std::optional<Trajectory>;
-using PointCloud2Buffer = std::optional<PointCloud2>;
-using PredictedObjectsBuffer = std::optional<PredictedObjects>;
-using DetectedObjectsBuffer = std::optional<DetectedObjects>;
-using TrackedObjectsBuffer = std::optional<TrackedObjects>;
-
+// Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime
+using MessageBuffer = std::optional<PublishedTime>;
+// We need to store the past AckermannControlCommands to analyze the first brake
+using ControlCommandBuffer = std::pair<std::vector<AckermannControlCommand>, MessageBuffer>;
 // Variant to store different types of buffers
-using BufferVariant = std::variant<
-  ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
-  DetectedObjectsBuffer, TrackedObjectsBuffer>;
+using MessageBufferVariant = std::variant<ControlCommandBuffer, MessageBuffer>;
 
 template <typename MessageType>
 struct SubscriberVariables
@@ -142,7 +121,7 @@ struct SearchZeroVelParams
 
 struct SearchEntityParams
 {
-  double search_radius;
+  double search_radius_offset;
 };
 
 // Place for the store the reaction parameter configuration (currently only for first brake)
@@ -159,33 +138,36 @@ class SubscriberBase
 {
 public:
   explicit SubscriberBase(
-    rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, geometry_msgs::msg::Pose entity_pose,
-    std::atomic<bool> & spawn_object_cmd);
+    rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic<bool> & spawn_object_cmd,
+    const EntityParams & entity_params);
 
   // 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();
+  std::optional<std::unordered_map<std::string, MessageBufferVariant>> getMessageBuffersMap();
   void reset();
 
 private:
   std::mutex mutex_;
 
+  // Init
   rclcpp::Node * node_;
   Odometry::ConstSharedPtr odometry_;
-  geometry_msgs::msg::Pose entity_pose_;
   std::atomic<bool> & spawn_object_cmd_;
+  EntityParams entity_params_;
 
   // Variables to be initialized in constructor
   ChainModules chain_modules_;
   ReactionParams reaction_params_{};
+  geometry_msgs::msg::Pose entity_pose_;
+  double entity_search_radius_;
 
   // Variants
   std::unordered_map<std::string, SubscriberVariablesVariant> subscriber_variables_map_;
-  std::unordered_map<std::string, BufferVariant> message_buffers_;
+  std::unordered_map<std::string, MessageBufferVariant> message_buffers_;
 
   // tf
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -194,14 +176,10 @@ class SubscriberBase
   // Functions
   void init_reaction_chains_and_params();
   bool init_subscribers();
-  bool search_pointcloud_near_entity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);
-  bool search_predicted_objects_near_entity(const PredictedObjects & predicted_objects);
-  bool search_detected_objects_near_entity(const DetectedObjects & detected_objects);
-  bool search_tracked_objects_near_entity(const TrackedObjects & tracked_objects);
-  void set_control_command_to_buffer(
-    std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
   std::optional<size_t> find_first_brake_idx(
     const std::vector<AckermannControlCommand> & cmd_array);
+  void set_control_command_to_buffer(
+    std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
 
   // Callbacks for modules are subscribed
   void on_control_command(

tools/reaction_analyzer/include/topic_publisher.hpp

@@ -17,12 +17,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rosbag2_cpp/reader.hpp>
+#include <tf2_eigen/tf2_eigen/tf2_eigen.hpp>
 #include <utils.hpp>
 
-#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp>
@@ -34,7 +31,9 @@
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <memory>
 #include <mutex>
@@ -75,6 +74,8 @@ enum class PublisherMessageType {
 
 struct TopicPublisherParams
 {
+  double dummy_perception_publisher_period;  // Only for planning_control mode
+  double spawned_pointcloud_sampling_distance;
   std::string path_bag_without_object;       // Path to the bag file without object
   std::string path_bag_with_object;          // Path to the bag file with object
   std::string pointcloud_publisher_type;     // Type of the pointcloud publisher
@@ -196,7 +197,8 @@ class TopicPublisher
 public:
   explicit TopicPublisher(
     rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd,
-    std::optional<rclcpp::Time> & spawn_cmd_time);
+    std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
+    const EntityParams & entity_params);
 
   ~TopicPublisher() = default;
   void reset();
@@ -206,13 +208,33 @@ class TopicPublisher
 
   // Initialized variables
   rclcpp::Node * node_;
+  RunningMode node_running_mode_;
   std::atomic<bool> & spawn_object_cmd_;
+  EntityParams entity_params_;
   std::optional<rclcpp::Time> & spawn_cmd_time_;  // Set by a publisher function when the
                                                   // spawn_object_cmd_ is true
 
+  // tf
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
   // Parameters
   TopicPublisherParams topic_publisher_params_;
 
+  // Variables planning_control mode
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_;
+  rclcpp::Publisher<PredictedObjects>::SharedPtr pub_predicted_objects_;
+  PointCloud2::SharedPtr entity_pointcloud_ptr_;
+  PredictedObjects::SharedPtr predicted_objects_ptr_;
+
+  // Variables perception_planning mode
+  PointcloudPublisherType pointcloud_publisher_type_;
+  std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
+  std::unordered_map<std::string, LidarOutputPair>
+    lidar_pub_variable_pair_map_;  // used to publish pointcloud_raw and pointcloud_raw_ex
+  bool is_object_spawned_message_published_{false};
+  std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
+
   // Functions
   void set_message_to_variable_map(
     const PublisherMessageType & message_type, const std::string & topic_name,
@@ -228,18 +250,12 @@ class TopicPublisher
       std::shared_ptr<PublisherVariables<PointCloud2>>,
       std::shared_ptr<PublisherVariables<PointCloud2>>> & lidar_output_pair_);
   void generic_message_publisher(const std::string & topic_name);
-
-  // Variables
-  PointcloudPublisherType pointcloud_publisher_type_;
-  std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
-  std::unordered_map<std::string, LidarOutputPair>
-    lidar_pub_variable_pair_map_;  // used to publish pointcloud_raw and pointcloud_raw_ex
-  bool is_object_spawned_message_published{false};
-  std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
+  void dummy_perception_publisher();  // Only for planning_control mode
 
   // Timers
   std::unordered_map<std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
   rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_;
+  rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
 };
 }  // namespace reaction_analyzer::topic_publisher