diff --git a/tools/reaction_analyzer/CMakeLists.txt b/tools/reaction_analyzer/CMakeLists.txt new file mode 100644 index 0000000000000..d94a5bd7a794e --- /dev/null +++ b/tools/reaction_analyzer/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.14) +project(reaction_analyzer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(reaction_analyzer SHARED + include/reaction_analyzer_node.hpp + src/reaction_analyzer_node.cpp + include/subscriber.hpp + src/subscriber.cpp + include/topic_publisher.hpp + src/topic_publisher.cpp + include/utils.hpp + src/utils.cpp +) + +target_include_directories(reaction_analyzer + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +target_link_libraries(reaction_analyzer + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(reaction_analyzer + PLUGIN "reaction_analyzer::ReactionAnalyzerNode" + EXECUTABLE reaction_analyzer_exe +) + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md new file mode 100644 index 0000000000000..f5a22aaf20176 --- /dev/null +++ b/tools/reaction_analyzer/README.md @@ -0,0 +1,229 @@ +# Reaction Analyzer + +## Description + +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) + +### Planning Control Mode + +In this mode, the reaction analyzer creates a dummy publisher for the PredictedObjects and PointCloud2 topics. In the +beginning of the test, it publishes the initial position of the ego vehicle and the goal position to set the test +environment. Then, it spawns a sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to +search reacted messages of the planning and control nodes in the pre-determined topics. When all the topics are reacted, +it calculates the reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes +with `spawn_cmd_time`, and it creates a csv file to store the results. + +### Perception Planning Mode + +In this mode, the reaction analyzer reads the rosbag files which are recorded from AWSIM, and it creates a topic +publisher for each topic inside the rosbag to replay the rosbag. It reads two rosbag files: `path_bag_without_object` +and `path_bag_with_object`. Firstly, it replays the `path_bag_without_object` to set the initial position of the ego +vehicle and the goal position. After `spawn_time_after_init` seconds , it replays the `path_bag_with_object` to spawn a +sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to search the reacted messages of +the perception and planning nodes in the pre-determined topics. When all the topics are reacted, it calculates the +reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes with `spawn_cmd_time`, and +it creates a csv file to store the results. + +#### Point Cloud Publisher Type + +To get better analyze for Perception & Sensing pipeline, the reaction analyzer can publish the point cloud messages in 3 +different ways: `async_header_sync_publish`, `sync_header_sync_publish` or `async_publish`. (`T` is the period of the +lidar's output) + +![PointcloudPublisherType.png](media%2FPointcloudPublisherType.png) + +- `async_header_sync_publish`: It publishes the point cloud messages synchronously with asynchronous header times. It + means that each of the lidar's output will be published at the same time, but the headers of the point cloud messages + includes different timestamps because of the phase difference. +- `sync_header_sync_publish`: It publishes the point cloud messages synchronously with synchronous header times. It + means that each of the lidar's output will be published at the same time, and the headers of the point cloud messages + includes the same timestamps. +- `async_publish`: It publishes the point cloud messages asynchronously. It means that each of the lidar's output will + be published at different times. + +## Usage + +The common parameters you need to define for both running modes are `output_file_path`, `test_iteration`, +and `reaction_chain` list. `output_file_path` is the output file path is the path where the results and statistics +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 + link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After + downloading, + extract the zip file and use its path as `[MAP_PATH]` in the following commands. + +#### Planning Control Mode + +- You need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters, + you can start to test with the following command: + +```bash +ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=planning_control vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit map_path:=[MAP_PATH] +``` + +After the command, the `simple_planning_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. + +#### Perception Planning Mode + +- Download the rosbag files from the Google Drive + 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`. +- 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_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 +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`: +- 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 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. + +**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. +- 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 +``` + +- 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. + +**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. + +## 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` +for each of the nodes. + +- `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. | +| `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. | +| `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. | +| `reaction_params.first_brake_params.debug_control_commands` | bool | Debug publish flag. | +| `reaction_params.first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. | +| `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. | +| `reaction_params.first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. | +| `reaction_params.search_zero_vel_params.max_looking_distance` | double | [m] Maximum looking distance for zero velocity on trajectory | +| `reaction_params.search_entity_params.search_radius` | double | [m] Searching radius for spawned entity. Distance between ego pose and entity pose. | +| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. | + +## Limitations + +- Reaction analyzer has some limitation like `PublisherMessageType`, `SubscriberMessageType` and `ReactionType`. It is + currently supporting following list: + +- **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` + +- **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` + +- **Reaction Types:** + - `FIRST_BRAKE` + - `SEARCH_ZERO_VEL` + - `SEARCH_ENTITY` + +## Future improvements + +- 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. diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp new file mode 100644 index 0000000000000..6dc3dd896dc92 --- /dev/null +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -0,0 +1,158 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef REACTION_ANALYZER_NODE_HPP_ +#define REACTION_ANALYZER_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer +{ +using autoware_adapi_v1_msgs::msg::LocalizationInitializationState; +using autoware_adapi_v1_msgs::msg::OperationModeState; +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; + +struct NodeParams +{ + std::string running_mode; + double timer_period; + std::string output_file_path; + size_t test_iteration; + double spawn_time_after_init; + double spawn_distance_threshold; + PoseParams initial_pose; + PoseParams goal_pose; + EntityParams entity_params; +}; + +class ReactionAnalyzerNode : public rclcpp::Node +{ +public: + explicit ReactionAnalyzerNode(rclcpp::NodeOptions node_options); + + Odometry::ConstSharedPtr odometry_ptr_; + +private: + std::mutex mutex_; + RunningMode node_running_mode_; + + // Parameters + NodeParams node_params_; + + // Initialization Variables + geometry_msgs::msg::Pose entity_pose_; + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_; + geometry_msgs::msg::PoseStamped goal_pose_; + + // Subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_route_state_; + rclcpp::Subscription::SharedPtr sub_localization_init_state_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Subscription::SharedPtr + sub_ground_truth_pose_; // Only for perception_planning mode + + // Publishers + rclcpp::Publisher::SharedPtr pub_initial_pose_; + rclcpp::Publisher::SharedPtr pub_goal_pose_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // Variables + std::vector pipeline_map_vector_; + std::optional last_test_environment_init_request_time_; + std::optional test_environment_init_time_; + std::optional spawn_cmd_time_; + std::atomic spawn_object_cmd_{false}; + std::atomic ego_initialized_{false}; + 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(); + void init_test_env( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const RouteState::ConstSharedPtr & route_state_ptr, + const OperationModeState::ConstSharedPtr & operation_mode_ptr, + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr); + bool init_ego( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time); + bool set_route( + const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time); + bool check_ego_init_correctly( + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr); + void call_operation_mode_service_without_response(); + void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose); + void calculate_results( + const std::map & message_buffers, + const rclcpp::Time & spawn_cmd_time); + void on_timer(); + void reset(); + + // Callbacks + void on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr); + void on_localization_initialization_state( + LocalizationInitializationState::ConstSharedPtr msg_ptr); + void on_route_state(RouteState::ConstSharedPtr msg_ptr); + void on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr); + void on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Client + rclcpp::Client::SharedPtr client_change_to_autonomous_; + + // Pointers + std::unique_ptr topic_publisher_ptr_; + std::unique_ptr subscriber_ptr_; + LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_; + RouteState::ConstSharedPtr current_route_state_ptr_; + OperationModeState::ConstSharedPtr operation_mode_ptr_; + PoseStamped::ConstSharedPtr ground_truth_pose_ptr_; +}; +} // namespace reaction_analyzer + +#endif // REACTION_ANALYZER_NODE_HPP_ diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp new file mode 100644 index 0000000000000..d14d41da545f7 --- /dev/null +++ b/tools/reaction_analyzer/include/subscriber.hpp @@ -0,0 +1,191 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SUBSCRIBER_HPP_ +#define SUBSCRIBER_HPP_ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer::subscriber +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_internal_msgs::msg::PublishedTime; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +// Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime +using MessageBuffer = std::optional; +// We need to store the past AckermannControlCommands to analyze the first brake +using ControlCommandBuffer = std::pair, MessageBuffer>; +// Variant to store different types of buffers +using MessageBufferVariant = std::variant; + +template +struct SubscriberVariables +{ + using ExactTimePolicy = message_filters::sync_policies::ExactTime; + + std::unique_ptr> sub1_; + std::unique_ptr> sub2_; + std::unique_ptr> synchronizer_; + // tmp: only for the messages who don't have header e.g. AckermannControlCommand + std::unique_ptr> cache_; +}; + +// Variant to create subscribers for different message types +using SubscriberVariablesVariant = std::variant< + SubscriberVariables, SubscriberVariables, + SubscriberVariables, SubscriberVariables, + SubscriberVariables, SubscriberVariables>; + +// The configuration of the topic to be subscribed which are defined in reaction_chain +struct TopicConfig +{ + std::string node_name; + std::string topic_address; + std::string time_debug_topic_address; + SubscriberMessageType message_type; +}; + +// Place for the reaction functions' parameter configuration +struct FirstBrakeParams +{ + bool debug_control_commands; + double control_cmd_buffer_time_interval; + size_t min_number_descending_order_control_cmd; + double min_jerk_for_brake_cmd; +}; + +struct SearchZeroVelParams +{ + double max_looking_distance; +}; + +struct SearchEntityParams +{ + double search_radius_offset; +}; + +// Place for the store the reaction parameter configuration (currently only for first brake) +struct ReactionParams +{ + FirstBrakeParams first_brake_params; + SearchZeroVelParams search_zero_vel_params; + SearchEntityParams search_entity_params; +}; + +using ChainModules = std::vector; + +class SubscriberBase +{ +public: + explicit SubscriberBase( + rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic & spawn_object_cmd, + const EntityParams & entity_params); + + std::optional> get_message_buffers_map(); + void reset(); + +private: + std::mutex mutex_; + + // Init + rclcpp::Node * node_; + Odometry::ConstSharedPtr odometry_; + std::atomic & 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_{0.0}; + + // Variants + std::map subscriber_variables_map_; + std::map message_buffers_; + + // tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Functions + void init_reaction_chains_and_params(); + bool init_subscribers(); + std::optional get_subscriber_variable( + const TopicConfig & topic_config); + std::optional find_first_brake_idx( + const std::vector & cmd_array); + void set_control_command_to_buffer( + std::vector & buffer, const AckermannControlCommand & cmd) const; + + // Callbacks for modules are subscribed + void on_control_command( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr); + void on_trajectory(const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr); + void on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_pointcloud(const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr); + void on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr); + void on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr); + void on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr); + void on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); +}; + +} // namespace reaction_analyzer::subscriber + +#endif // SUBSCRIBER_HPP_ diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp new file mode 100644 index 0000000000000..c6d3a90650884 --- /dev/null +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -0,0 +1,249 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TOPIC_PUBLISHER_HPP_ +#define TOPIC_PUBLISHER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer::topic_publisher +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +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 + double pointcloud_publisher_period; // Period of the pointcloud publisher + bool publish_only_pointcloud_with_object; // Publish only pointcloud with object for only + // perception pipeline debug purpose make it true. +}; + +enum class PointcloudPublisherType { + ASYNC_PUBLISHER = 0, // Asynchronous publisher + SYNC_HEADER_SYNC_PUBLISHER = 1, // Synchronous publisher with header synchronization + ASYNC_HEADER_SYNC_PUBLISHER = 2, // Asynchronous publisher with header synchronization +}; + +/** + * @brief Message type template struct for the variables of the Publisher. + */ +template +struct PublisherVariables +{ + std::chrono::milliseconds period_ms{0}; + typename MessageType::SharedPtr empty_area_message; + typename MessageType::SharedPtr object_spawned_message; + typename rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; +}; + +/** + * @brief Struct for accessing the variables of the Publisher. + */ +struct PublisherVarAccessor +{ + // Template struct to check if a type has a header member. + template > + struct HasHeader : std::false_type + { + }; + + template + struct HasHeader> : std::true_type + { + }; + + // Template struct to check if a type has a stamp member. + template > + struct HasStamp : std::false_type + { + }; + + template + struct HasStamp> : std::true_type + { + }; + + template + void publish_with_current_time( + const PublisherVariables & publisher_var, const rclcpp::Time & current_time, + const bool is_object_spawned) const + { + std::unique_ptr msg_to_be_published = std::make_unique(); + + if (is_object_spawned) { + *msg_to_be_published = *publisher_var.object_spawned_message; + } else { + *msg_to_be_published = *publisher_var.empty_area_message; + } + if constexpr (HasHeader::value) { + msg_to_be_published->header.stamp = current_time; + } else if constexpr (HasStamp::value) { + msg_to_be_published->stamp = current_time; + } + publisher_var.publisher->publish(std::move(msg_to_be_published)); + } + + template + void set_period(T & publisher_var, std::chrono::milliseconds new_period) + { + publisher_var.period_ms = new_period; + } + + template + std::chrono::milliseconds get_period(const T & publisher_var) const + { + return publisher_var.period_ms; + } + + template + std::shared_ptr get_empty_area_message(const T & publisher_var) const + { + return std::static_pointer_cast(publisher_var.empty_area_message); + } + + template + std::shared_ptr get_object_spawned_message(const T & publisher_var) const + { + return std::static_pointer_cast(publisher_var.object_spawned_message); + } +}; + +using PublisherVariablesVariant = std::variant< + PublisherVariables, PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables>; + +using LidarOutputPair = std::pair< + std::shared_ptr>, + std::shared_ptr>>; + +class TopicPublisher +{ +public: + explicit TopicPublisher( + rclcpp::Node * node, std::atomic & spawn_object_cmd, std::atomic & ego_initialized, + std::optional & spawn_cmd_time, const RunningMode & node_running_mode, + const EntityParams & entity_params); + + void reset(); + +private: + std::mutex mutex_; + + // Initialized variables + rclcpp::Node * node_; + RunningMode node_running_mode_; + std::atomic & spawn_object_cmd_; + std::atomic & ego_initialized_; // used for planning_control mode because if pointcloud is + // published before ego is initialized, it causes crash + EntityParams entity_params_; + std::optional & spawn_cmd_time_; // Set by a publisher function when the + // spawn_object_cmd_ is true + + // tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Parameters + TopicPublisherParams topic_publisher_params_; + + // Variables planning_control mode + rclcpp::Publisher::SharedPtr pub_pointcloud_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + PointCloud2::SharedPtr entity_pointcloud_ptr_; + PredictedObjects::SharedPtr predicted_objects_ptr_; + + // Variables perception_planning mode + PointcloudPublisherType pointcloud_publisher_type_; + std::map topic_publisher_map_; + std::map + lidar_pub_variable_pair_map_; // used to publish pointcloud_raw and pointcloud_raw_ex + bool is_object_spawned_message_published_{false}; + std::shared_ptr one_shot_timer_shared_ptr_; + + // Functions + template + void set_message( + const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message, + const bool is_empty_area_message); + void set_period(const std::map> & time_map); + void set_publishers_and_timers_to_variable(); + void set_timers_for_pointcloud_msgs( + const std::map> & pointcloud_variables_map); + bool check_publishers_initialized_correctly(); + void init_rosbag_publishers(); + 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< + std::shared_ptr>, + std::shared_ptr>> & lidar_output_pair_); + void generic_message_publisher(const std::string & topic_name); + void dummy_perception_publisher(); // Only for planning_control mode + + // Timers + std::map pointcloud_publish_timers_map_; + rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_; + rclcpp::TimerBase::SharedPtr dummy_perception_timer_; +}; +} // namespace reaction_analyzer::topic_publisher + +#endif // TOPIC_PUBLISHER_HPP_ diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp new file mode 100644 index 0000000000000..da1defc03f34c --- /dev/null +++ b/tools/reaction_analyzer/include/utils.hpp @@ -0,0 +1,363 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool. +namespace reaction_analyzer +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_internal_msgs::msg::PublishedTime; +using sensor_msgs::msg::PointCloud2; + +/** + * @brief A pair containing the ReactionPair. + * The first element is name of the node that published the PublishedTime msg. + * The second element is the PublishedTime msg itself + */ +using ReactionPair = std::pair; + +/** + * @brief A map containing the pipeline and the reaction pairs. + * The key is the time at which the pipeline was executed. + * The value is a vector of ReactionPair. + */ +using PipelineMap = std::map>; + +/** + * @brief A vector containing the pipeline and the reaction pairs. + * The first element is the time at which the pipeline was executed. + * The second element is a vector of ReactionPair. + */ +using TimestampedReactionPairsVector = + std::vector>>; + +/** + * @brief Enum for the different types of messages that can be published. + */ +enum class PublisherMessageType { + UNKNOWN = 0, + CAMERA_INFO = 1, + IMAGE = 2, + POINTCLOUD2 = 3, + POSE_WITH_COVARIANCE_STAMPED = 4, + POSE_STAMPED = 5, + ODOMETRY = 6, + IMU = 7, + CONTROL_MODE_REPORT = 8, + GEAR_REPORT = 9, + HAZARD_LIGHTS_REPORT = 10, + STEERING_REPORT = 11, + TURN_INDICATORS_REPORT = 12, + VELOCITY_REPORT = 13, +}; + +/** + * @brief Enum for the different types of messages that can be subscribed to. + */ +enum class SubscriberMessageType { + UNKNOWN = 0, + ACKERMANN_CONTROL_COMMAND = 1, + TRAJECTORY = 2, + POINTCLOUD2 = 3, + DETECTED_OBJECTS = 4, + PREDICTED_OBJECTS = 5, + TRACKED_OBJECTS = 6, +}; + +/** + * @brief Enum for the different types of reactions that can be analyzed. + */ +enum class ReactionType { + UNKNOWN = 0, + FIRST_BRAKE = 1, + SEARCH_ZERO_VEL = 2, + SEARCH_ENTITY = 3, +}; + +/** + * @brief Enum for the different running modes of the Reaction Analyzer. + */ +enum class RunningMode { + PerceptionPlanning = 0, + PlanningControl = 1, +}; + +/** + * @brief Structs 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 Struct containing statistics of the latencies. + */ +struct LatencyStats +{ + double min; + double max; + double mean; + double median; + double std_dev; +}; + +/** + * @brief Convert string to SubscriberMessageType. + */ +SubscriberMessageType get_subscriber_message_type(const std::string & message_type); + +/** + * @brief Convert string to PublisherMessageType. + */ +PublisherMessageType get_publisher_message_type(const std::string & message_type); + +/** + * @brief Get the PublisherMessageType for a given topic. + */ +PublisherMessageType get_publisher_message_type_for_topic( + const std::vector & topics, const std::string & topic_name); + +/** + * @brief Convert string to ReactionType. + */ +ReactionType get_reaction_type(const std::string & reaction_type); + +/** + * @brief Calculates the statistics of a vector of latencies. + * @param latency_vec The vector of latencies. + * @return The statistics of the latencies. + */ +LatencyStats calculate_statistics(const std::vector & latency_vec); + +/** + * @brief Generates a UUID message from a string. + * @param input The string to generate the UUID from. + * @return The generated UUID message. + */ +unique_identifier_msgs::msg::UUID generate_uuid_msg(const std::string & input); + +/** + * @brief Generate pose from PoseParams. + * @param pose_params + * @return Pose + */ +geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params); + +/** + * @brief Generate entity pose from entity params. + * @param entity_params + * @return Pose + */ +geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params); + +/** + * @brief Generate entity pose from entity params. + * @param entity_params + * @return Pose + */ +double calculate_entity_search_radius(const EntityParams & entity_params); + +/** + * @brief Create a pointcloud from entity params. + * @param entity_params + * @param pointcloud_sampling_distance + * @return PointCloud2::SharedPtr + */ +PointCloud2::SharedPtr create_entity_pointcloud_ptr( + const EntityParams & entity_params, const double pointcloud_sampling_distance); + +/** + * @brief Create a predicted objects message from entity params. + * @param entity_params + * @return PredictedObjects::SharedPtr + */ +PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityParams & entity_params); + +/** + * @brief Creates a subscription option with a new callback group. + * + * @param node A pointer to the node for which the subscription options are being created. + * @return The created SubscriptionOptions. + */ +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. + * + * @param str The string to be split. + * @param delimiter The delimiter to split the string by. + * @return A vector of strings, each of which is a segment of the original string split by the + * delimiter. + */ +std::vector 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. + * + * @param traj The trajectory to search. + * @param curr_id The index of the current point in the trajectory. + * @param distance The distance to search for a point. + * @return The index of the point that is at least the specified distance away from the current + * point. + */ +size_t get_index_after_distance( + const Trajectory & traj, const size_t curr_id, const double distance); + +/** + * @brief Search for a pointcloud near the pose. + * @param pcl_pointcloud, pose, search_radius + * @return bool + */ +bool search_pointcloud_near_pose( + const pcl::PointCloud & pcl_pointcloud, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * + * @brief Search for a predicted object near the pose. + * @param predicted_objects, pose, search_radius + * @return bool + */ +bool search_predicted_objects_near_pose( + const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * @brief Search for a detected object near the pose. + * @param detected_objects, pose, search_radius + * @return bool + */ +bool search_detected_objects_near_pose( + const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * @brief Search for a tracked object near the pose. + * @param tracked_objects, pose, search_radius + * @return bool + */ +bool search_tracked_objects_near_pose( + const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); + +/** + * Calculates the time difference in milliseconds between two rclcpp::Time instances. + * + * @param start The start time. + * @param end The end time. + * @return The time difference in milliseconds as a double. + */ +double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & end); + +/** + * @brief Converts a PipelineMap to a PipelineMapVector. + * + * @param pipelineMap The PipelineMap to be converted. + * @return The PipelineMapVector that is equivalent to the PipelineMap. + */ +TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector( + const PipelineMap & pipelineMap); + +/** + * @brief Writes the results to a file. + * + * @param node A pointer to the node for which the results are being written. + * @param output_file_path The path to the file where the results will be written. + * @param node_running_mode The running mode of the node. + * @param pipeline_map_vector The vector of PipelineMap containing the results to be written. + */ +void write_results( + rclcpp::Node * node, const std::string & output_file_path, const RunningMode & node_running_mode, + const std::vector & pipeline_map_vector); +} // namespace reaction_analyzer + +#endif // UTILS_HPP_ diff --git a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml new file mode 100644 index 0000000000000..b0b1d4fb9bf2e --- /dev/null +++ b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/reaction_analyzer/media/PointcloudPublisherType.png b/tools/reaction_analyzer/media/PointcloudPublisherType.png new file mode 100644 index 0000000000000..597b73318bcac Binary files /dev/null and b/tools/reaction_analyzer/media/PointcloudPublisherType.png differ diff --git a/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png b/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png new file mode 100644 index 0000000000000..cb0f2dd8577fc Binary files /dev/null and b/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png differ diff --git a/tools/reaction_analyzer/media/sc1-awsim.png b/tools/reaction_analyzer/media/sc1-awsim.png new file mode 100644 index 0000000000000..6bdc104a93b87 Binary files /dev/null and b/tools/reaction_analyzer/media/sc1-awsim.png differ diff --git a/tools/reaction_analyzer/media/sc1-rviz.png b/tools/reaction_analyzer/media/sc1-rviz.png new file mode 100644 index 0000000000000..ae7c6e882bf98 Binary files /dev/null and b/tools/reaction_analyzer/media/sc1-rviz.png differ diff --git a/tools/reaction_analyzer/media/sc2-awsim.png b/tools/reaction_analyzer/media/sc2-awsim.png new file mode 100644 index 0000000000000..8224b516897be Binary files /dev/null and b/tools/reaction_analyzer/media/sc2-awsim.png differ diff --git a/tools/reaction_analyzer/media/sc2-rviz.png b/tools/reaction_analyzer/media/sc2-rviz.png new file mode 100644 index 0000000000000..8f4f5feae8ba3 Binary files /dev/null and b/tools/reaction_analyzer/media/sc2-rviz.png differ diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml new file mode 100644 index 0000000000000..05081eac01751 --- /dev/null +++ b/tools/reaction_analyzer/package.xml @@ -0,0 +1,49 @@ + + + + reaction_analyzer + 1.0.0 + Analyzer that measures reaction times of the nodes + + Berkay Karaman + + Apache License 2.0 + + Berkay Karaman + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_internal_msgs + eigen + libpcl-all-dev + message_filters + motion_utils + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + rosbag2_cpp + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + + + ament_cmake + + diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml new file mode 100644 index 0000000000000..62c1191bd345a --- /dev/null +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -0,0 +1,117 @@ +/**: + ros__parameters: + timer_period: 0.033 # s + test_iteration: 10 + output_file_path: + 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: 81546.984375 + y: 50011.96875 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 11.1130405 + goal_pose: + x: 81643.0703125 + y: 50029.8828125 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 13.12 + entity_params: + 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: /rosbag2_awsim_labs/rosbag2_awsim_labs.db3 + path_bag_with_object: /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 + publish_only_pointcloud_with_object: false # use it only for debug purposes. If true, only pointclouds with object will be published + spawned_pointcloud_sampling_distance: 0.1 # m for planning_control mode + dummy_perception_publisher_period: 0.1 # s for planning_control mode + subscriber: + reaction_params: + first_brake_params: + debug_control_commands: false + control_cmd_buffer_time_interval: 1.0 # s + min_number_descending_order_control_cmd: 3 + min_jerk_for_brake_cmd: 0.3 # m/s^3 + search_zero_vel_params: + max_looking_distance: 15.0 # m + search_entity_params: + search_radius_offset: 0.0 # m + reaction_chain: + obstacle_cruise_planner: + topic_name: /planning/scenario_planning/lane_driving/trajectory + time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + scenario_selector: + topic_name: /planning/scenario_planning/scenario_selector/trajectory + time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + motion_velocity_smoother: + topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory + time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + planning_validator: + topic_name: /planning/scenario_planning/trajectory + time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + trajectory_follower: + topic_name: /control/trajectory_follower/control_cmd + time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time + message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + vehicle_cmd_gate: + topic_name: /control/command/control_cmd + time_debug_topic_name: /control/command/control_cmd/debug/published_time + message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + common_ground_filter: + topic_name: /perception/obstacle_segmentation/single_frame/pointcloud + time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud/debug/published_time + message_type: sensor_msgs/msg/PointCloud2 + occupancy_grid_map_outlier: + topic_name: /perception/obstacle_segmentation/pointcloud + time_debug_topic_name: /perception/obstacle_segmentation/pointcloud/debug/published_time + message_type: sensor_msgs/msg/PointCloud2 + multi_object_tracker: + topic_name: /perception/object_recognition/tracking/near_objects + time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/TrackedObjects + lidar_centerpoint: + topic_name: /perception/object_recognition/detection/centerpoint/objects + time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + obstacle_pointcloud_based_validator: + topic_name: /perception/object_recognition/detection/centerpoint/validation/objects + time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + decorative_tracker_merger: + topic_name: /perception/object_recognition/tracking/objects + time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/TrackedObjects + detected_object_feature_remover: + topic_name: /perception/object_recognition/detection/clustering/objects + time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + detection_by_tracker: + topic_name: /perception/object_recognition/detection/detection_by_tracker/objects + time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + object_lanelet_filter: + topic_name: /perception/object_recognition/detection/objects + time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + map_based_prediction: + topic_name: /perception/object_recognition/objects + time_debug_topic_name: /perception/object_recognition/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/PredictedObjects diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp new file mode 100644 index 0000000000000..53313f28eb4d9 --- /dev/null +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -0,0 +1,442 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "reaction_analyzer_node.hpp" + +#include +#include + +namespace reaction_analyzer +{ + +void ReactionAnalyzerNode::on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + operation_mode_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_route_state(RouteState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + current_route_state_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + odometry_ptr_ = msg_ptr; +} + +void ReactionAnalyzerNode::on_localization_initialization_state( + LocalizationInitializationState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + initialization_state_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + ground_truth_pose_ptr_ = std::move(msg_ptr); +} + +ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options) +: Node("reaction_analyzer_node", node_options.automatically_declare_parameters_from_overrides(true)) +{ + using std::placeholders::_1; + + node_params_.running_mode = get_parameter("running_mode").as_string(); + + // set running mode + if (node_params_.running_mode == "planning_control") { + node_running_mode_ = RunningMode::PlanningControl; + } else if (node_params_.running_mode == "perception_planning") { + node_running_mode_ = RunningMode::PerceptionPlanning; + } else { + RCLCPP_ERROR(get_logger(), "Invalid running mode. Node couldn't be initialized. Failed."); + 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_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double(); + node_params_.spawn_distance_threshold = get_parameter("spawn_distance_threshold").as_double(); + + // Position parameters + 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( + "input/kinematics", 1, std::bind(&ReactionAnalyzerNode::on_vehicle_pose, this, _1), + create_subscription_options(this)); + sub_localization_init_state_ = create_subscription( + "input/localization_initialization_state", rclcpp::QoS(1).transient_local(), + std::bind(&ReactionAnalyzerNode::on_localization_initialization_state, this, _1), + create_subscription_options(this)); + sub_route_state_ = create_subscription( + "input/routing_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::on_route_state, this, _1), create_subscription_options(this)); + sub_operation_mode_ = create_subscription( + "input/operation_mode_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::on_operation_mode_state, this, _1), + create_subscription_options(this)); + + pub_goal_pose_ = create_publisher("output/goal", rclcpp::QoS(1)); + pub_marker_ = create_publisher("~/debug", 10); + + init_analyzer_variables(); + + if (node_running_mode_ == RunningMode::PlanningControl) { + pub_initial_pose_ = create_publisher( + "output/initialpose", rclcpp::QoS(1)); + + client_change_to_autonomous_ = + create_client("service/change_to_autonomous"); + + } else if (node_running_mode_ == RunningMode::PerceptionPlanning) { + sub_ground_truth_pose_ = create_subscription( + "input/ground_truth_pose", rclcpp::QoS{1}, + std::bind(&ReactionAnalyzerNode::on_ground_truth_pose, this, _1), + create_subscription_options(this)); + } + + topic_publisher_ptr_ = std::make_unique( + this, spawn_object_cmd_, ego_initialized_, spawn_cmd_time_, node_running_mode_, + node_params_.entity_params); + + // initialize the odometry before init the subscriber + odometry_ptr_ = std::make_shared(); + // Load the subscriber to listen the topics for reactions + subscriber_ptr_ = std::make_unique( + this, odometry_ptr_, spawn_object_cmd_, node_params_.entity_params); + + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(node_params_.timer_period)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ReactionAnalyzerNode::on_timer, this)); +} + +void ReactionAnalyzerNode::on_timer() +{ + mutex_.lock(); + const auto current_odometry_ptr = odometry_ptr_; + const auto initialization_state_ptr = initialization_state_ptr_; + const auto route_state_ptr = current_route_state_ptr_; + const auto operation_mode_ptr = operation_mode_ptr_; + const auto ground_truth_pose_ptr = ground_truth_pose_ptr_; + const auto spawn_cmd_time = spawn_cmd_time_; + mutex_.unlock(); + + // Init the test environment + if (!test_environment_init_time_) { + init_test_env( + initialization_state_ptr, route_state_ptr, operation_mode_ptr, ground_truth_pose_ptr, + current_odometry_ptr); + return; + } + + pub_marker_->publish(entity_debug_marker_); + + // 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 modules reacted + const auto message_buffers = subscriber_ptr_->get_message_buffers_map(); + + 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(); + } +} + +void ReactionAnalyzerNode::spawn_obstacle(const geometry_msgs::msg::Point & ego_pose) +{ + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + rclcpp::Duration time_diff = this->now() - test_environment_init_time_.value(); + if (time_diff > rclcpp::Duration::from_seconds(node_params_.spawn_time_after_init)) { + if (!spawn_object_cmd_) { + spawn_object_cmd_ = true; + RCLCPP_INFO(this->get_logger(), "Spawn command is sent."); + } + } + } else { + if ( + tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) < + node_params_.spawn_distance_threshold) { + if (!spawn_object_cmd_) { + spawn_object_cmd_ = true; + RCLCPP_INFO(this->get_logger(), "Spawn command is sent."); + } + } + } +} + +void ReactionAnalyzerNode::calculate_results( + const std::map & message_buffers, + const rclcpp::Time & spawn_cmd_time) +{ + // Map the reaction times w.r.t its header time to categorize it + PipelineMap pipeline_map; + + { + // set the spawn_cmd_time as the first reaction pair + ReactionPair reaction_pair; + reaction_pair.first = "spawn_cmd_time"; + reaction_pair.second.header.stamp = spawn_cmd_time; + reaction_pair.second.published_stamp = spawn_cmd_time; + pipeline_map[reaction_pair.second.header.stamp].emplace_back(reaction_pair); + } + + for (const auto & [key, variant] : message_buffers) { + ReactionPair reaction_pair; + if (auto * control_message = std::get_if(&variant)) { + if (control_message->second) { + reaction_pair.first = key; + reaction_pair.second = control_message->second.value(); + } + } else if (auto * general_message = std::get_if(&variant)) { + if (general_message->has_value()) { + reaction_pair.first = key; + reaction_pair.second = general_message->value(); + } + } + pipeline_map[reaction_pair.second.header.stamp].emplace_back(reaction_pair); + } + pipeline_map_vector_.emplace_back(pipeline_map); + test_iteration_count_++; +} + +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) { + init_pose_.pose.pose = pose_params_to_pose(node_params_.initial_pose); + } +} + +void ReactionAnalyzerNode::init_test_env( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const RouteState::ConstSharedPtr & route_state_ptr, + const OperationModeState::ConstSharedPtr & operation_mode_ptr, + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr) +{ + const auto current_time = this->now(); + + // Initialize the test environment + constexpr double initialize_call_period = 1.0; // sec + if ( + !last_test_environment_init_request_time_ || + (current_time - last_test_environment_init_request_time_.value()).seconds() >= + initialize_call_period) { + last_test_environment_init_request_time_ = current_time; + + // Pose initialization of the ego + is_initialization_requested = !is_initialization_requested + ? init_ego(initialization_state_ptr, odometry_ptr, current_time) + : is_initialization_requested; + + if ( + is_initialization_requested && + initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { + is_initialization_requested = false; + return; + } + + // Check is position initialized accurately, if node is running in perception_planning mode + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + if (!check_ego_init_correctly(ground_truth_pose_ptr, odometry_ptr)) return; + } + + // Set route + is_route_set_ = !is_route_set_ ? set_route(route_state_ptr, current_time) : is_route_set_; + + if (!is_route_set_) { + return; + } + + // if node is running PlanningControl mode, change ego to Autonomous mode. + if (node_running_mode_ == RunningMode::PlanningControl) { + // change to autonomous + if (operation_mode_ptr && operation_mode_ptr->mode != OperationModeState::AUTONOMOUS) { + call_operation_mode_service_without_response(); + } + } + ego_initialized_ = true; + + const bool is_ready = + (is_initialization_requested && is_route_set_ && + (operation_mode_ptr->mode == OperationModeState::AUTONOMOUS || + node_running_mode_ == RunningMode::PerceptionPlanning)); + if (is_ready) { + test_environment_init_time_ = this->now(); + } + } +} + +void ReactionAnalyzerNode::call_operation_mode_service_without_response() +{ + auto req = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), "client request"); + + if (!client_change_to_autonomous_->service_is_ready()) { + RCLCPP_INFO(this->get_logger(), "client is unavailable"); + return; + } + + client_change_to_autonomous_->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + this->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); +} + +bool ReactionAnalyzerNode::init_ego( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time) +{ + // Pose initialization of the ego + if (initialization_state_ptr) { + 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) { + RCLCPP_WARN_ONCE(get_logger(), "Odometry is not received. Waiting for odometry..."); + return false; + } + return true; + } + return false; +} + +bool ReactionAnalyzerNode::set_route( + const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time) +{ + if (route_state_ptr) { + if (route_state_ptr->state != RouteState::SET) { + // publish goal pose + goal_pose_.header.stamp = current_time; + goal_pose_.header.frame_id = "map"; + pub_goal_pose_->publish(goal_pose_); + return false; + } + return true; + } + return false; +} + +bool ReactionAnalyzerNode::check_ego_init_correctly( + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr) +{ + if (!ground_truth_pose_ptr) { + RCLCPP_WARN( + get_logger(), "Ground truth pose is not received. Waiting for Ground truth pose..."); + return false; + } + if (!odometry_ptr) { + RCLCPP_WARN(get_logger(), "Odometry is not received. Waiting for odometry..."); + return false; + } + + constexpr double deviation_threshold = 0.1; + const auto deviation = + tier4_autoware_utils::calcPoseDeviation(ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); + const bool is_position_initialized_correctly = deviation.longitudinal < deviation_threshold && + deviation.lateral < deviation_threshold && + deviation.yaw < deviation_threshold; + if (!is_position_initialized_correctly) { + RCLCPP_ERROR( + get_logger(), + "Deviation between ground truth position and ego position is high. Node is shutting " + "down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f", + deviation.longitudinal, deviation.lateral, deviation.yaw); + rclcpp::shutdown(); + } + return true; +} + +void ReactionAnalyzerNode::reset() +{ + if (test_iteration_count_ >= node_params_.test_iteration) { + write_results(this, node_params_.output_file_path, node_running_mode_, pipeline_map_vector_); + RCLCPP_INFO(get_logger(), "%zu Tests are finished. Node shutting down.", test_iteration_count_); + rclcpp::shutdown(); + return; + } + // reset the variables + is_initialization_requested = false; + is_route_set_ = false; + test_environment_init_time_ = std::nullopt; + last_test_environment_init_request_time_ = std::nullopt; + spawn_object_cmd_ = false; + if (topic_publisher_ptr_) { + topic_publisher_ptr_->reset(); + } + std::lock_guard lock(mutex_); + spawn_cmd_time_ = std::nullopt; + subscriber_ptr_->reset(); + ego_initialized_ = false; + RCLCPP_INFO(this->get_logger(), "Test - %zu is done, resetting..", test_iteration_count_); +} +} // namespace reaction_analyzer + +#include + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(reaction_analyzer::ReactionAnalyzerNode) diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp new file mode 100644 index 0000000000000..8c735c42b9cd5 --- /dev/null +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -0,0 +1,996 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "subscriber.hpp" + +#include +#include +#include + +namespace reaction_analyzer::subscriber +{ + +SubscriberBase::SubscriberBase( + rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic & spawn_object_cmd, + const EntityParams & entity_params) +: node_(node), + odometry_(odometry), + spawn_object_cmd_(spawn_object_cmd), + entity_params_(entity_params) +{ + // init tf + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // init reaction parameters and chain configuration + init_reaction_chains_and_params(); + init_subscribers(); +} + +void SubscriberBase::init_reaction_chains_and_params() +{ + // Init Chains: get the topic addresses and message types of the modules in chain + { + const auto param_key = std::string("subscriber.reaction_chain"); + const auto module_names = node_->list_parameters({param_key}, 3).prefixes; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + TopicConfig tmp; + tmp.node_name = splitted_name.back(); + tmp.topic_address = node_->get_parameter(module_name + ".topic_name").as_string(); + tmp.time_debug_topic_address = + node_->get_parameter_or(module_name + ".time_debug_topic_name", std::string("")); + tmp.message_type = get_subscriber_message_type( + node_->get_parameter(module_name + ".message_type").as_string()); + if (tmp.message_type != SubscriberMessageType::UNKNOWN) { + chain_modules_.emplace_back(tmp); + } else { + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for module name: %s, skipping..", + tmp.node_name.c_str()); + } + } + } + + // Init Params: get the parameters for the reaction functions + { + const auto param_key = std::string("subscriber.reaction_params"); + const auto module_names = node_->list_parameters({param_key}, 3).prefixes; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + const auto type = get_reaction_type(splitted_name.back()); + switch (type) { + case ReactionType::FIRST_BRAKE: { + reaction_params_.first_brake_params.debug_control_commands = + node_->get_parameter(module_name + ".debug_control_commands").as_bool(); + reaction_params_.first_brake_params.control_cmd_buffer_time_interval = + node_->get_parameter(module_name + ".control_cmd_buffer_time_interval").as_double(); + reaction_params_.first_brake_params.min_number_descending_order_control_cmd = + node_->get_parameter(module_name + ".min_number_descending_order_control_cmd").as_int(); + reaction_params_.first_brake_params.min_jerk_for_brake_cmd = + node_->get_parameter(module_name + ".min_jerk_for_brake_cmd").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), + "First brake parameters are set: debug_control_commands %s, " + "control_cmd_buffer_time_interval %f, min_number_descending_order_control_cmd %zu, " + "min_jerk_for_brake_cmd %f", + reaction_params_.first_brake_params.debug_control_commands ? "true" : "false", + reaction_params_.first_brake_params.control_cmd_buffer_time_interval, + reaction_params_.first_brake_params.min_number_descending_order_control_cmd, + reaction_params_.first_brake_params.min_jerk_for_brake_cmd); + break; + } + case ReactionType::SEARCH_ZERO_VEL: { + reaction_params_.search_zero_vel_params.max_looking_distance = + node_->get_parameter(module_name + ".max_looking_distance").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), "Search Zero Vel parameters are set: max_looking_distance %f", + reaction_params_.search_zero_vel_params.max_looking_distance); + break; + } + case ReactionType::SEARCH_ENTITY: { + reaction_params_.search_entity_params.search_radius_offset = + node_->get_parameter(module_name + ".search_radius_offset").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), "Search Entity parameters are set: search_radius_offset %f", + reaction_params_.search_entity_params.search_radius_offset); + break; + } + default: + RCLCPP_WARN( + node_->get_logger(), "Unknown reaction type for module name: %s, skipping..", + splitted_name.back().c_str()); + break; + } + } + } + + // Init variables + { + entity_pose_ = create_entity_pose(entity_params_); + entity_search_radius_ = calculate_entity_search_radius(entity_params_) + + reaction_params_.search_entity_params.search_radius_offset; + } +} + +bool SubscriberBase::init_subscribers() +{ + if (chain_modules_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No module to initialize subscribers, failed."); + return false; + } + + for (const auto & module : chain_modules_) { + if (module.message_type == SubscriberMessageType::UNKNOWN) { + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for topic_config name: %s, skipping..", + module.node_name.c_str()); + } + auto subscriber_var = get_subscriber_variable(module); + if (!subscriber_var) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to initialize subscriber for module name: %s", + module.node_name.c_str()); + return false; + } + subscriber_variables_map_[module.node_name] = std::move(subscriber_var.value()); + } + return true; +} + +std::optional> SubscriberBase::get_message_buffers_map() +{ + std::lock_guard lock(mutex_); + if (message_buffers_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No message buffers are initialized."); + return std::nullopt; + } + + // Check all reacted or not + bool all_reacted = true; + for (const auto & [key, variant] : message_buffers_) { + if (auto * control_message = std::get_if(&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(&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; + } + } + } + if (!all_reacted) { + return std::nullopt; + } + return message_buffers_; +} + +void SubscriberBase::reset() +{ + std::lock_guard lock(mutex_); + message_buffers_.clear(); +} + +// Callbacks + +void SubscriberBase::on_control_command( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr) +{ + std::lock_guard lock(mutex_); + auto & variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); + variant = buffer; + } + auto & cmd_buffer = std::get(variant); + if (cmd_buffer.second) { + // reacted + return; + } + set_control_command_to_buffer(cmd_buffer.first, *msg_ptr); + if (!spawn_object_cmd_) return; + + const auto brake_idx = find_first_brake_idx(cmd_buffer.first); + if (brake_idx) { + const auto brake_cmd = cmd_buffer.first.at(brake_idx.value()); + + // TODO(brkay54): update here if message_filters package add support for the messages which + // does not have header + const auto & subscriber_variant = + std::get>(subscriber_variables_map_[node_name]); + + // check if the cache was initialized or not, if there is, use it to set the published time + if (subscriber_variant.cache_) { + // use cache to get the published time + const auto published_time_vec = + subscriber_variant.cache_->getSurroundingInterval(brake_cmd.stamp, node_->now()); + if (!published_time_vec.empty()) { + for (const auto & published_time : published_time_vec) { + if (published_time->header.stamp == brake_cmd.stamp) { + cmd_buffer.second = *published_time; + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + return; + } + } + RCLCPP_ERROR( + node_->get_logger(), "Published time couldn't found for the node: %s", node_name.c_str()); + + } else { + RCLCPP_ERROR( + node_->get_logger(), "Published time vector is empty for the node: %s", + node_name.c_str()); + } + } else { + cmd_buffer.second->header.stamp = brake_cmd.stamp; + cmd_buffer.second->published_stamp = brake_cmd.stamp; + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + } + } +} + +void SubscriberBase::on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!current_odometry_ptr || !spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + msg_ptr->points, current_odometry_ptr->pose.pose.position); + + const auto nearest_objects_seg_idx = + motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); + + const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( + msg_ptr->points, nearest_seg_idx, nearest_objects_seg_idx); + + if (zero_vel_idx) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!current_odometry_ptr || !spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + msg_ptr->points, current_odometry_ptr->pose.pose.position); + + // find the target index which we will search for zero velocity + auto tmp_target_idx = get_index_after_distance( + *msg_ptr, nearest_seg_idx, reaction_params_.search_zero_vel_params.max_looking_distance); + if (tmp_target_idx == msg_ptr->points.size() - 1) { + tmp_target_idx = msg_ptr->points.size() - 2; // Last trajectory points might be zero velocity + } + const auto target_idx = tmp_target_idx; + const auto zero_vel_idx = + motion_utils::searchZeroVelocityIndex(msg_ptr->points, nearest_seg_idx, target_idx); + + if (zero_vel_idx) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points); + + pcl::PointCloud pcl_pointcloud; + pcl::fromROSMsg(transformed_points, pcl_pointcloud); + + if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} +void SubscriberBase::on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points); + + pcl::PointCloud pcl_pointcloud; + pcl::fromROSMsg(transformed_points, pcl_pointcloud); + + if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} +void SubscriberBase::on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + // transform objects + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + // transform objects + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_tracked_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_tracked_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +std::optional SubscriberBase::get_subscriber_variable( + const TopicConfig & topic_config) +{ + switch (topic_config.message_type) { + case SubscriberMessageType::ACKERMANN_CONTROL_COMMAND: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + // If not empty, user should define a time debug topic + // NOTE: Because message_filters package does not support the messages without headers, we + // can not use the synchronizer. After we reacted, we are going to use the cache + // of the both PublishedTime and AckermannControlCommand subscribers to find the messages + // which have same header time. + + std::function callback = + [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + this->on_control_command(topic_config.node_name, ptr); + }; + subscriber_variable.sub1_ = + std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + constexpr int cache_size = 5; + subscriber_variable.cache_ = std::make_unique>( + *subscriber_variable.sub2_, cache_size); + + } else { + std::function callback = + [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + this->on_control_command(topic_config.node_name, ptr); + }; + + subscriber_variable.sub1_ = + std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::TRAJECTORY: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const Trajectory::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_trajectory(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const Trajectory::ConstSharedPtr & msg) { + this->on_trajectory(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::POINTCLOUD2: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const PointCloud2::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_pointcloud(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::SensorDataQoS().get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const PointCloud2::ConstSharedPtr & msg) { + this->on_pointcloud(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::PREDICTED_OBJECTS: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const PredictedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_predicted_objects(topic_config.node_name, ptr, published_time_ptr); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const PredictedObjects::ConstSharedPtr & msg) { + this->on_predicted_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::DETECTED_OBJECTS: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const DetectedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_detected_objects(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const DetectedObjects::ConstSharedPtr & msg) { + this->on_detected_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::TRACKED_OBJECTS: { + SubscriberVariables subscriber_variable; + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const TrackedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_tracked_objects(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const TrackedObjects::ConstSharedPtr & msg) { + this->on_tracked_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + default: + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for topic_config name: %s, skipping..", + topic_config.node_name.c_str()); + return std::nullopt; + } +} + +std::optional SubscriberBase::find_first_brake_idx( + const std::vector & cmd_array) +{ + if ( + cmd_array.size() < + reaction_params_.first_brake_params.min_number_descending_order_control_cmd) { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Control command buffer size is less than the minimum required size for first brake check"); + return {}; + } + + for (size_t i = 0; + i < cmd_array.size() - + reaction_params_.first_brake_params.min_number_descending_order_control_cmd + 1; + ++i) { + size_t decreased_cmd_counter = 1; // because # of the decreased cmd = iteration + 1 + for (size_t j = i; j < cmd_array.size() - 1; ++j) { + const auto & cmd_first = cmd_array.at(j).longitudinal; + const auto & cmd_second = cmd_array.at(j + 1).longitudinal; + constexpr double jerk_time_epsilon = 0.001; + const auto jerk = + abs(cmd_second.acceleration - cmd_first.acceleration) / + std::max( + (rclcpp::Time(cmd_second.stamp) - rclcpp::Time(cmd_first.stamp)).seconds(), + jerk_time_epsilon); + + if ( + (cmd_second.acceleration < cmd_first.acceleration) && + (jerk > reaction_params_.first_brake_params.min_jerk_for_brake_cmd)) { + decreased_cmd_counter++; + } else { + break; + } + } + if ( + decreased_cmd_counter < + static_cast( + reaction_params_.first_brake_params.min_number_descending_order_control_cmd)) + continue; + if (reaction_params_.first_brake_params.debug_control_commands) { + std::stringstream ss; + + // debug print to show the first brake command in the all control commands + for (size_t k = 0; k < cmd_array.size(); ++k) { + if (k == i + 1) { + ss << "First Brake(" << cmd_array.at(k).longitudinal.acceleration << ") "; + } else { + ss << cmd_array.at(k).longitudinal.acceleration << " "; + } + } + + RCLCPP_INFO(node_->get_logger(), "%s", ss.str().c_str()); + } + return i + 1; + } + return {}; +} + +void SubscriberBase::set_control_command_to_buffer( + std::vector & buffer, const AckermannControlCommand & cmd) const +{ + const auto last_cmd_time = cmd.stamp; + if (!buffer.empty()) { + for (auto itr = buffer.begin(); itr != buffer.end();) { + const auto expired = (rclcpp::Time(last_cmd_time) - rclcpp::Time(itr->stamp)).seconds() > + reaction_params_.first_brake_params.control_cmd_buffer_time_interval; + + if (expired) { + itr = buffer.erase(itr); + continue; + } + + itr++; + } + } + buffer.emplace_back(cmd); +} +} // namespace reaction_analyzer::subscriber diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp new file mode 100644 index 0000000000000..8a66bf9e33911 --- /dev/null +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -0,0 +1,536 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "topic_publisher.hpp" + +#include +#include + +namespace reaction_analyzer::topic_publisher +{ + +TopicPublisher::TopicPublisher( + rclcpp::Node * node, std::atomic & spawn_object_cmd, std::atomic & ego_initialized, + std::optional & spawn_cmd_time, const RunningMode & node_running_mode, + const EntityParams & entity_params) +: node_(node), + node_running_mode_(node_running_mode), + spawn_object_cmd_(spawn_object_cmd), + ego_initialized_(ego_initialized), + entity_params_(entity_params), + spawn_cmd_time_(spawn_cmd_time) +{ + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + // get perception_planning mode parameters + topic_publisher_params_.path_bag_with_object = + node_->get_parameter("topic_publisher.path_bag_with_object").as_string(); + topic_publisher_params_.path_bag_without_object = + node_->get_parameter("topic_publisher.path_bag_without_object").as_string(); + topic_publisher_params_.pointcloud_publisher_type = + node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_type") + .as_string(); + topic_publisher_params_.pointcloud_publisher_period = + node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_period") + .as_double(); + topic_publisher_params_.publish_only_pointcloud_with_object = + node_ + ->get_parameter("topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object") + .as_bool(); + + // set pointcloud publisher type + if (topic_publisher_params_.pointcloud_publisher_type == "sync_header_sync_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER; + } else if (topic_publisher_params_.pointcloud_publisher_type == "async_header_sync_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER; + } else if (topic_publisher_params_.pointcloud_publisher_type == "async_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_PUBLISHER; + } else { + RCLCPP_ERROR(node_->get_logger(), "Invalid pointcloud_publisher_type"); + rclcpp::shutdown(); + return; + } + + // Init the publishers which will read the messages from the rosbag + init_rosbag_publishers(); + } else if (node_running_mode_ == RunningMode::PlanningControl) { + // init tf + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // get parameters + topic_publisher_params_.dummy_perception_publisher_period = + node_->get_parameter("topic_publisher.dummy_perception_publisher_period").as_double(); + topic_publisher_params_.spawned_pointcloud_sampling_distance = + node_->get_parameter("topic_publisher.spawned_pointcloud_sampling_distance").as_double(); + + // init the messages that will be published to spawn the object + entity_pointcloud_ptr_ = create_entity_pointcloud_ptr( + entity_params_, topic_publisher_params_.spawned_pointcloud_sampling_distance); + predicted_objects_ptr_ = create_entity_predicted_objects_ptr(entity_params_); + + // init the publishers + pub_pointcloud_ = + node_->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); + pub_predicted_objects_ = + node_->create_publisher("output/objects", rclcpp::QoS(1)); + + // init dummy perception publisher + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.dummy_perception_publisher_period)); + + dummy_perception_timer_ = rclcpp::create_timer( + node_, node_->get_clock(), period_ns, + std::bind(&TopicPublisher::dummy_perception_publisher, this)); + } else { + RCLCPP_ERROR(node_->get_logger(), "Invalid running mode"); + rclcpp::shutdown(); + return; + } +} + +void TopicPublisher::pointcloud_messages_sync_publisher(const PointcloudPublisherType type) +{ + const auto current_time = node_->now(); + const bool is_object_spawned = spawn_object_cmd_; + + switch (type) { + case PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER: { + PublisherVarAccessor accessor; + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + accessor.publish_with_current_time( + *publisher_var_pair.second.first, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *publisher_var_pair.second.second, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + } + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } + break; + } + case PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER: { + PublisherVarAccessor accessor; + const auto period_pointcloud_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); + const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size(); + + size_t counter = 0; + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + const auto header_time = + current_time - std::chrono::nanoseconds(counter * phase_dif.count()); + accessor.publish_with_current_time( + *publisher_var_pair.second.first, header_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *publisher_var_pair.second.second, header_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + counter++; + } + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } + break; + } + default: + break; + } +} + +void TopicPublisher::pointcloud_messages_async_publisher( + const std::pair< + std::shared_ptr>, + std::shared_ptr>> & lidar_output_pair_) +{ + PublisherVarAccessor accessor; + const auto current_time = node_->now(); + const bool is_object_spawned = spawn_object_cmd_; + accessor.publish_with_current_time( + *lidar_output_pair_.first, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *lidar_output_pair_.second, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } +} + +void TopicPublisher::generic_message_publisher(const std::string & topic_name) +{ + PublisherVarAccessor accessor; + const bool is_object_spawned = spawn_object_cmd_; + const auto current_time = node_->now(); + const auto & publisher_variant = topic_publisher_map_[topic_name]; + + std::visit( + [&](const auto & var) { + accessor.publish_with_current_time(var, current_time, is_object_spawned); + }, + publisher_variant); +} + +void TopicPublisher::dummy_perception_publisher() +{ + if (!ego_initialized_) { + return; // do not publish anything if ego is not initialized + } + if (!spawn_object_cmd_) { + // do not spawn it, send empty pointcloud + pcl::PointCloud pcl_empty; + PointCloud2 empty_pointcloud; + PredictedObjects empty_predicted_objects; + pcl::toROSMsg(pcl_empty, empty_pointcloud); + + const auto current_time = node_->now(); + empty_pointcloud.header.frame_id = "base_link"; + empty_pointcloud.header.stamp = current_time; + + empty_predicted_objects.header.frame_id = "map"; + empty_predicted_objects.header.stamp = current_time; + + pub_pointcloud_->publish(empty_pointcloud); + pub_predicted_objects_->publish(empty_predicted_objects); + } else { + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "base_link", "map", node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to look up transform from map to base_link"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *entity_pointcloud_ptr_, transformed_points); + const auto current_time = node_->now(); + + transformed_points.header.frame_id = "base_link"; + transformed_points.header.stamp = current_time; + + predicted_objects_ptr_->header.frame_id = "map"; + predicted_objects_ptr_->header.stamp = current_time; + + pub_pointcloud_->publish(transformed_points); + pub_predicted_objects_->publish(*predicted_objects_ptr_); + if (!is_object_spawned_message_published_) { + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + is_object_spawned_message_published_ = true; + } + } +} + +void TopicPublisher::reset() +{ + is_object_spawned_message_published_ = false; +} + +void TopicPublisher::init_rosbag_publishers() +{ + // read messages without object + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_without_object, true); + + // read messages with object + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_with_object, false); + + // before create publishers and timers, check all the messages are correctly initialized with + // their conjugate messages. + if (check_publishers_initialized_correctly()) { + RCLCPP_INFO(node_->get_logger(), "Messages are initialized correctly"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Messages are not initialized correctly"); + rclcpp::shutdown(); + } + set_publishers_and_timers_to_variable(); +} + +template +void TopicPublisher::set_message( + const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message, + const bool is_empty_area_message) +{ + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message.serialized_data); + + // Deserialize the message + auto deserialized_message = std::make_shared(); + serialization.deserialize_message(&extracted_serialized_msg, &*deserialized_message); + auto & publisher_variant = topic_publisher_map_[topic_name]; + + if (!std::holds_alternative>(publisher_variant)) { + publisher_variant = PublisherVariables{}; + } + + auto & publisher_variable = std::get>(publisher_variant); + + if (is_empty_area_message) { + if (!publisher_variable.empty_area_message) { + publisher_variable.empty_area_message = deserialized_message; + } + } else { + if (!publisher_variable.object_spawned_message) { + publisher_variable.object_spawned_message = deserialized_message; + } + } +} + +void TopicPublisher::set_period(const std::map> & time_map) +{ + for (auto & topic_pair : time_map) { + auto timestamps_tmp = topic_pair.second; + + // Sort the timestamps + std::sort(timestamps_tmp.begin(), timestamps_tmp.end()); + + // Then proceed with the frequency calculation + std::string topic_name = topic_pair.first; + if (timestamps_tmp.size() > 1) { + int64_t total_time_diff_ns = 0; + + // Accumulate the differences in nanoseconds + for (size_t i = 1; i < timestamps_tmp.size(); ++i) { + total_time_diff_ns += (timestamps_tmp[i] - timestamps_tmp[i - 1]).nanoseconds(); + } + + // Conversion to std::chrono::milliseconds + auto total_duration_ns = std::chrono::nanoseconds(total_time_diff_ns); + auto period_ms = std::chrono::duration_cast(total_duration_ns) / + (timestamps_tmp.size() - 1); + + PublisherVariablesVariant & publisher_variant = topic_publisher_map_[topic_name]; + PublisherVarAccessor accessor; + + std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisher_variant); + } + } +} + +void TopicPublisher::set_publishers_and_timers_to_variable() +{ + std::map> + pointcloud_variables_map; // temp map for pointcloud publishers + + // initialize timers and message publishers except pointcloud messages + for (auto & [topic_name, variant] : topic_publisher_map_) { + PublisherVarAccessor accessor; + const auto & topic_ref = topic_name; + const auto period_ns = std::chrono::duration( + std::visit([&](const auto & var) { return accessor.get_period(var); }, variant)); + + // Dynamically create the correct publisher type based on the topic + std::visit( + [&](auto & var) { + using MessageType = typename decltype(var.empty_area_message)::element_type; + + if constexpr ( + std::is_same_v || + std::is_same_v || + std::is_same_v) { + var.publisher = node_->create_publisher(topic_ref, rclcpp::SensorDataQoS()); + } else { + // For other message types, use the QoS setting depth of 1 + var.publisher = node_->create_publisher(topic_ref, rclcpp::QoS(1)); + } + }, + variant); + + // Conditionally create the timer based on the message type, if message type is not + // PointCloud2 + std::visit( + [&](auto & var) { + using MessageType = typename decltype(var.empty_area_message)::element_type; + + if constexpr (!std::is_same_v) { + var.timer = node_->create_wall_timer( + period_ns, [this, topic_ref]() { this->generic_message_publisher(topic_ref); }); + } else { + // For PointCloud2, Store the variables in a temporary map + pointcloud_variables_map[topic_ref] = var; + } + }, + variant); + } + + // To be able to publish pointcloud messages with async, I need to create a timer for each lidar + // output. So different operations are needed for pointcloud messages. + set_timers_for_pointcloud_msgs(pointcloud_variables_map); +} + +void TopicPublisher::set_timers_for_pointcloud_msgs( + const std::map> & pointcloud_variables_map) +{ + // Set the point cloud publisher timers + if (pointcloud_variables_map.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No pointcloud publishers found!"); + rclcpp::shutdown(); + } + + // Arrange the PointCloud2 variables w.r.t. the lidars' name + for (auto & [topic_name, pointcloud_variant] : pointcloud_variables_map) { + const auto lidar_name = split(topic_name, '/').at(3); + + if (lidar_pub_variable_pair_map_.find(lidar_name) == lidar_pub_variable_pair_map_.end()) { + lidar_pub_variable_pair_map_[lidar_name] = std::make_pair( + std::make_shared>(pointcloud_variant), nullptr); + } else { + if (lidar_pub_variable_pair_map_[lidar_name].second) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Lidar name: " << lidar_name << " is already used by another pointcloud publisher"); + rclcpp::shutdown(); + } + lidar_pub_variable_pair_map_[lidar_name].second = + std::make_shared>(pointcloud_variant); + } + } + + // Create the timer(s) to publish PointCloud2 Messages + const auto period_pointcloud_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); + + if (pointcloud_publisher_type_ != PointcloudPublisherType::ASYNC_PUBLISHER) { + // Create 1 timer to publish all PointCloud2 messages + pointcloud_sync_publish_timer_ = node_->create_wall_timer(period_pointcloud_ns, [this]() { + this->pointcloud_messages_sync_publisher(this->pointcloud_publisher_type_); + }); + } else { + // Create multiple timers which will run with a phase difference + const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size(); + + // Create a timer to create phase difference bw timers which will be created for each lidar + // topics + auto one_shot_timer = node_->create_wall_timer(phase_dif, [this, period_pointcloud_ns]() { + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + const auto & lidar_name = publisher_var_pair.first; + const auto & publisher_var = publisher_var_pair.second; + if ( + pointcloud_publish_timers_map_.find(lidar_name) == pointcloud_publish_timers_map_.end()) { + auto periodic_timer = node_->create_wall_timer( + period_pointcloud_ns, + [this, publisher_var]() { this->pointcloud_messages_async_publisher(publisher_var); }); + pointcloud_publish_timers_map_[lidar_name] = periodic_timer; + return; + } + } + one_shot_timer_shared_ptr_->cancel(); + }); + one_shot_timer_shared_ptr_ = one_shot_timer; + } +} + +bool TopicPublisher::check_publishers_initialized_correctly() +{ + // check messages are correctly initialized or not from rosbags + for (const auto & [topic_name, variant] : topic_publisher_map_) { + PublisherVarAccessor accessor; + auto empty_area_message = + std::visit([&](const auto & var) { return accessor.get_empty_area_message(var); }, variant); + auto object_spawned_message = std::visit( + [&](const auto & var) { return accessor.get_object_spawned_message(var); }, variant); + + if (!empty_area_message) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Empty area message couldn't found in the topic named: " << topic_name); + return false; + } + if (!object_spawned_message) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Object spawned message couldn't found in the topic named: " << topic_name); + return false; + } + } + return true; +} + +void TopicPublisher::init_rosbag_publisher_buffer( + const std::string & bag_path, const bool is_empty_area_message) +{ + rosbag2_cpp::Reader reader; + + try { + reader.open(bag_path); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error opening bag file: " << e.what()); + rclcpp::shutdown(); + return; + } + + const auto & topics = reader.get_metadata().topics_with_message_count; + + while (reader.has_next()) { + auto bag_message = reader.read_next(); + const auto current_topic = bag_message->topic_name; + + const auto message_type = get_publisher_message_type_for_topic(topics, current_topic); + + if (message_type == PublisherMessageType::UNKNOWN) { + continue; + } + if (message_type == PublisherMessageType::CAMERA_INFO) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::IMAGE) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POINTCLOUD2) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POSE_STAMPED) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::ODOMETRY) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::IMU) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::CONTROL_MODE_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::GEAR_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::HAZARD_LIGHTS_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::STEERING_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::TURN_INDICATORS_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::VELOCITY_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } + } + reader.close(); +} +} // namespace reaction_analyzer::topic_publisher diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp new file mode 100644 index 0000000000000..4a6110322440e --- /dev/null +++ b/tools/reaction_analyzer/src/utils.cpp @@ -0,0 +1,589 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils.hpp" + +namespace reaction_analyzer +{ +SubscriberMessageType get_subscriber_message_type(const std::string & message_type) +{ + if (message_type == "autoware_auto_control_msgs/msg/AckermannControlCommand") { + return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND; + } + if (message_type == "autoware_auto_planning_msgs/msg/Trajectory") { + return SubscriberMessageType::TRAJECTORY; + } + if (message_type == "sensor_msgs/msg/PointCloud2") { + return SubscriberMessageType::POINTCLOUD2; + } + if (message_type == "autoware_auto_perception_msgs/msg/PredictedObjects") { + return SubscriberMessageType::PREDICTED_OBJECTS; + } + if (message_type == "autoware_auto_perception_msgs/msg/DetectedObjects") { + return SubscriberMessageType::DETECTED_OBJECTS; + } + if (message_type == "autoware_auto_perception_msgs/msg/TrackedObjects") { + return SubscriberMessageType::TRACKED_OBJECTS; + } + return SubscriberMessageType::UNKNOWN; +} + +PublisherMessageType get_publisher_message_type(const std::string & message_type) +{ + if (message_type == "sensor_msgs/msg/PointCloud2") { + return PublisherMessageType::POINTCLOUD2; + } + if (message_type == "sensor_msgs/msg/CameraInfo") { + return PublisherMessageType::CAMERA_INFO; + } + if (message_type == "sensor_msgs/msg/Image") { + return PublisherMessageType::IMAGE; + } + if (message_type == "geometry_msgs/msg/PoseWithCovarianceStamped") { + return PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED; + } + if (message_type == "geometry_msgs/msg/PoseStamped") { + return PublisherMessageType::POSE_STAMPED; + } + if (message_type == "nav_msgs/msg/Odometry") { + return PublisherMessageType::ODOMETRY; + } + if (message_type == "sensor_msgs/msg/Imu") { + return PublisherMessageType::IMU; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/ControlModeReport") { + return PublisherMessageType::CONTROL_MODE_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/GearReport") { + return PublisherMessageType::GEAR_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/HazardLightsReport") { + return PublisherMessageType::HAZARD_LIGHTS_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/SteeringReport") { + return PublisherMessageType::STEERING_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport") { + return PublisherMessageType::TURN_INDICATORS_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/VelocityReport") { + return PublisherMessageType::VELOCITY_REPORT; + } + return PublisherMessageType::UNKNOWN; +} + +PublisherMessageType get_publisher_message_type_for_topic( + const std::vector & topics, const std::string & topic_name) +{ + auto it = std::find_if(topics.begin(), topics.end(), [&topic_name](const auto & topic) { + return topic.topic_metadata.name == topic_name; + }); + if (it != topics.end()) { + return get_publisher_message_type(it->topic_metadata.type); // Return the message type if found + } + return PublisherMessageType::UNKNOWN; +} + +ReactionType get_reaction_type(const std::string & reaction_type) +{ + if (reaction_type == "first_brake_params") { + return ReactionType::FIRST_BRAKE; + } + if (reaction_type == "search_zero_vel_params") { + return ReactionType::SEARCH_ZERO_VEL; + } + if (reaction_type == "search_entity_params") { + return ReactionType::SEARCH_ENTITY; + } + return ReactionType::UNKNOWN; +} + +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + 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 split(const std::string & str, char delimiter) +{ + std::vector elements; + std::stringstream ss(str); + std::string item; + while (std::getline(ss, item, delimiter)) { + elements.push_back(item); + } + 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) +{ + const TrajectoryPoint & curr_p = traj.points.at(curr_id); + + size_t target_id = curr_id; + double current_distance = 0.0; + for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { + current_distance = tier4_autoware_utils::calcDistance3d(traj.points.at(traj_id), curr_p); + if (current_distance >= distance) { + break; + } + target_id = traj_id; + } + return target_id; +} + +double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & end) +{ + const auto duration = end - start; + + const auto duration_ns = duration.to_chrono(); + return static_cast(duration_ns.count()) / 1e6; +} + +TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector( + const PipelineMap & pipelineMap) +{ + std::vector>> sorted_vector; + + for (const auto & entry : pipelineMap) { + auto sorted_reactions = entry.second; + // Sort the vector of ReactionPair based on the published stamp + std::sort( + sorted_reactions.begin(), sorted_reactions.end(), + [](const ReactionPair & a, const ReactionPair & b) { + return rclcpp::Time(a.second.published_stamp) < rclcpp::Time(b.second.published_stamp); + }); + + // Add to the vector as a tuple + sorted_vector.emplace_back(std::make_tuple(entry.first, sorted_reactions)); + } + + // Sort the vector of tuples by rclcpp::Time + std::sort(sorted_vector.begin(), sorted_vector.end(), [](const auto & a, const auto & b) { + return std::get<0>(a) < std::get<0>(b); + }); + + return sorted_vector; +} + +unique_identifier_msgs::msg::UUID generate_uuid_msg(const std::string & input) +{ + static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); + const auto uuid = generate_uuid(input); + + unique_identifier_msgs::msg::UUID uuid_msg; + std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); + return uuid_msg; +} + +geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params) +{ + geometry_msgs::msg::Pose entity_pose; + entity_pose.position.x = entity_params.x; + entity_pose.position.y = entity_params.y; + entity_pose.position.z = entity_params.z; + + tf2::Quaternion entity_q_orientation; + entity_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(entity_params.roll), + tier4_autoware_utils::deg2rad(entity_params.pitch), + tier4_autoware_utils::deg2rad(entity_params.yaw)); + entity_pose.orientation = tf2::toMsg(entity_q_orientation); + return entity_pose; +} + +geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = pose_params.x; + pose.position.y = pose_params.y; + pose.position.z = pose_params.z; + + tf2::Quaternion pose_q_orientation; + pose_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(pose_params.roll), + tier4_autoware_utils::deg2rad(pose_params.pitch), + tier4_autoware_utils::deg2rad(pose_params.yaw)); + pose.orientation = tf2::toMsg(pose_q_orientation); + return pose; +} + +PointCloud2::SharedPtr create_entity_pointcloud_ptr( + const EntityParams & entity_params, const double pointcloud_sampling_distance) +{ + pcl::PointCloud point_cloud; + tf2::Quaternion entity_q_orientation; + + entity_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(entity_params.roll), + tier4_autoware_utils::deg2rad(entity_params.pitch), + tier4_autoware_utils::deg2rad(entity_params.yaw)); + tf2::Transform tf(entity_q_orientation); + const auto origin = tf2::Vector3(entity_params.x, entity_params.y, entity_params.z); + tf.setOrigin(origin); + + const double it_x = entity_params.x_l / pointcloud_sampling_distance; + const double it_y = entity_params.y_l / pointcloud_sampling_distance; + const double it_z = entity_params.z_l / pointcloud_sampling_distance; + + // Sample the box and rotate + for (int i = 0; i <= it_z; ++i) { + for (int j = 0; j <= it_y; ++j) { + for (int k = 0; k <= it_x; ++k) { + const double p_x = -entity_params.x_l / 2 + k * pointcloud_sampling_distance; + const double p_y = -entity_params.y_l / 2 + j * pointcloud_sampling_distance; + const double p_z = -entity_params.z_l / 2 + i * pointcloud_sampling_distance; + const auto tmp = tf2::Vector3(p_x, p_y, p_z); + tf2::Vector3 data_out = tf * tmp; + point_cloud.emplace_back(pcl::PointXYZ( + static_cast(data_out.x()), static_cast(data_out.y()), + static_cast(data_out.z()))); + } + } + } + PointCloud2::SharedPtr entity_pointcloud_ptr; + entity_pointcloud_ptr = std::make_shared(); + pcl::toROSMsg(point_cloud, *entity_pointcloud_ptr); + return entity_pointcloud_ptr; +} + +PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityParams & entity_params) +{ + unique_identifier_msgs::msg::UUID uuid_msg; + + PredictedObject obj; + const auto entity_pose = create_entity_pose(entity_params); + geometry_msgs::msg::Vector3 dimension; + dimension.set__x(entity_params.x_l); + dimension.set__y(entity_params.y_l); + dimension.set__z(entity_params.z_l); + obj.shape.set__dimensions(dimension); + + obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.existence_probability = 1.0; + obj.kinematics.initial_pose_with_covariance.pose = entity_pose; + + autoware_auto_perception_msgs::msg::PredictedPath path; + path.confidence = 1.0; + path.path.emplace_back(entity_pose); + obj.kinematics.predicted_paths.emplace_back(path); + + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + classification.probability = 1.0; + obj.classification.emplace_back(classification); + obj.set__object_id(generate_uuid_msg("test_obstacle")); + + PredictedObjects pred_objects; + pred_objects.objects.emplace_back(obj); + return std::make_shared(pred_objects); +} + +double calculate_entity_search_radius(const EntityParams & entity_params) +{ + return std::sqrt( + std::pow(entity_params.x_l, 2) + std::pow(entity_params.y_l, 2) + + std::pow(entity_params.z_l, 2)) / + 2.0; +} + +bool search_pointcloud_near_pose( + const pcl::PointCloud & pcl_pointcloud, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), + [pose, search_radius](const auto & point) { + return tier4_autoware_utils::calcDistance3d(pose.position, point) <= search_radius; + }); +} + +bool search_predicted_objects_near_pose( + const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [pose, search_radius](const PredictedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.initial_pose_with_covariance.pose.position) <= + search_radius; + }); + ; +} + +bool search_detected_objects_near_pose( + const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + detected_objects.objects.begin(), detected_objects.objects.end(), + [pose, search_radius](const DetectedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.pose_with_covariance.pose.position) <= + search_radius; + }); +} + +bool search_tracked_objects_near_pose( + const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + tracked_objects.objects.begin(), tracked_objects.objects.end(), + [pose, search_radius](const TrackedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.pose_with_covariance.pose.position) <= + search_radius; + }); +} + +LatencyStats calculate_statistics(const std::vector & latency_vec) +{ + LatencyStats stats{0.0, 0.0, 0.0, 0.0, 0.0}; + stats.max = *max_element(latency_vec.begin(), latency_vec.end()); + stats.min = *min_element(latency_vec.begin(), latency_vec.end()); + + const double sum = std::accumulate(latency_vec.begin(), latency_vec.end(), 0.0); + stats.mean = sum / static_cast(latency_vec.size()); + + std::vector sorted_latencies = latency_vec; + std::sort(sorted_latencies.begin(), sorted_latencies.end()); + stats.median = sorted_latencies.size() % 2 == 0 + ? (sorted_latencies[sorted_latencies.size() / 2 - 1] + + sorted_latencies[sorted_latencies.size() / 2]) / + 2 + : sorted_latencies[sorted_latencies.size() / 2]; + + const double sq_sum = + std::inner_product(latency_vec.begin(), latency_vec.end(), latency_vec.begin(), 0.0); + stats.std_dev = + std::sqrt(sq_sum / static_cast(latency_vec.size()) - stats.mean * stats.mean); + return stats; +} + +void write_results( + rclcpp::Node * node, const std::string & output_file_path, const RunningMode & node_running_mode, + const std::vector & pipeline_map_vector) +{ + // 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 << output_file_path; + if (!output_file_path.empty() && 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"; + + // open file + std::ofstream file(ss.str()); + + // Check if the file was opened successfully + if (!file.is_open()) { + RCLCPP_ERROR_ONCE(node->get_logger(), "Failed to open file: %s", ss.str().c_str()); + return; + } + + // tmp map to store latency results for statistics + std::map>> tmp_latency_map; + + size_t test_count = 0; + for (const auto & pipeline_map : pipeline_map_vector) { + test_count++; + // convert pipeline_map to vector of tuples + file << "Test " << test_count << "\n"; + const auto sorted_results_vector = convert_pipeline_map_to_sorted_vector(pipeline_map); + const auto spawn_cmd_time = std::get<0>(*sorted_results_vector.begin()); + + for (size_t i = 0; i < sorted_results_vector.size(); ++i) { + const auto & [pipeline_header_time, pipeline_reactions] = sorted_results_vector[i]; + + // total time pipeline lasts + file << "Pipeline - " << i << ","; + + // pipeline nodes + for (const auto & [node_name, reaction] : pipeline_reactions) { + file << node_name << ","; + } + + file << "\nNode - Pipeline - Total Latency [ms],"; + + for (size_t j = 0; j < pipeline_reactions.size(); ++j) { + const auto & reaction = pipeline_reactions[j].second; + const auto & node_name = pipeline_reactions[j].first; + if (j == 0) { + const auto node_latency = + calculate_time_diff_ms(reaction.header.stamp, reaction.published_stamp); + const auto pipeline_latency = + calculate_time_diff_ms(pipeline_header_time, reaction.published_stamp); + 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( + std::make_tuple(node_latency, pipeline_latency, total_latency)); + } else { + const auto & prev_reaction = pipeline_reactions[j - 1].second; + const auto node_latency = + calculate_time_diff_ms(prev_reaction.published_stamp, reaction.published_stamp); + const auto pipeline_latency = + calculate_time_diff_ms(pipeline_header_time, reaction.published_stamp); + 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( + std::make_tuple(node_latency, pipeline_latency, total_latency)); + } + } + file << "\n"; + } + } + + // write statistics + + file << "\nStatistics\n"; + file << "Node " + "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 node_latencies; + std::vector pipeline_latencies; + std::vector total_latencies; + + // Extract latencies + for (const auto & latencies : latency_vec) { + 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_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()); +} +} // namespace reaction_analyzer