diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md index 8c8510559c678..f5a22aaf20176 100644 --- a/tools/reaction_analyzer/README.md +++ b/tools/reaction_analyzer/README.md @@ -54,6 +54,11 @@ and `reaction_chain` list. `output_file_path` is the output file path is the pat will be stored. `test_iteration` defines how many tests will be performed. The `reaction_chain` list is the list of the pre-defined topics you want to measure their reaction times. +**IMPORTANT:** Ensure the `reaction_chain` list is correctly defined: + +- For `perception_planning` mode, **do not** define `Control` nodes. +- For `planning_control` mode, **do not** define `Perception` nodes. + ### Prepared Test Environment - Download the demonstration test map from the @@ -119,7 +124,8 @@ ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehic - After the EGO stopped in desired position, please localize the dummy obstacle by using the traffic controller. You can control the traffic by pressing `ESC` button. -**After localize EGO and dummy vehicle, we should write the positions of these entities in the map frame in `reaction_analyzer.param.yaml`. To achieve this:** +**After localize EGO and dummy vehicle, we should write the positions of these entities in the map frame +in `reaction_analyzer.param.yaml`. To achieve this:** - Get initialization pose from `/awsim/ground_truth/vehicle/pose` topic. - Get entity params from `/perception/object_recognition/objects` topic. @@ -146,10 +152,12 @@ to run Autoware while recording.** ## Results The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each -pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, `Pipeline Latency`, and `Total Latency` +pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, `Pipeline Latency`, +and `Total Latency` for each of the nodes. -- `Node Latency`: The time difference between previous and current node's reaction timestamps. +- `Node Latency`: The time difference between previous and current node's reaction timestamps. If it is the first node + in the pipeline, it is same as `Pipeline Latency`. - `Pipeline Latency`: The time difference between published time of the message and pipeline header time. - `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent timestamp. diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index cce073a7f3367..6dc3dd896dc92 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -93,6 +93,7 @@ class ReactionAnalyzerNode : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr pub_initial_pose_; rclcpp::Publisher::SharedPtr pub_goal_pose_; + rclcpp::Publisher::SharedPtr pub_marker_; // Variables std::vector pipeline_map_vector_; @@ -104,6 +105,7 @@ class ReactionAnalyzerNode : public rclcpp::Node bool is_initialization_requested{false}; bool is_route_set_{false}; size_t test_iteration_count_{0}; + visualization_msgs::msg::Marker entity_debug_marker_; // Functions void init_analyzer_variables(); diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp index 150aa00fef1c1..da1defc03f34c 100644 --- a/tools/reaction_analyzer/include/utils.hpp +++ b/tools/reaction_analyzer/include/utils.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,7 @@ #include #include +#include #include #include #include @@ -253,6 +255,15 @@ PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityPara */ rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node); +/** + * @brief Creates a visualization marker for a polyhedron based on the provided entity parameters. + * + * @param params The parameters of the entity for which the marker is to be created. It includes the + * position, orientation, and dimensions of the entity. + * @return The created visualization marker for the polyhedron. + */ +visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params); + /** * @brief Splits a string by a given delimiter. * @@ -263,6 +274,14 @@ rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node); */ std::vector 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. diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index 2e47c19ae0ab2..53313f28eb4d9 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -68,9 +68,15 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options) return; } + node_params_.output_file_path = get_parameter("output_file_path").as_string(); + // Check if the output file path is valid + if (!does_folder_exist(node_params_.output_file_path)) { + RCLCPP_ERROR(get_logger(), "Output file path is not valid. Node couldn't be initialized."); + return; + } + node_params_.timer_period = get_parameter("timer_period").as_double(); node_params_.test_iteration = get_parameter("test_iteration").as_int(); - node_params_.output_file_path = get_parameter("output_file_path").as_string(); node_params_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double(); node_params_.spawn_distance_threshold = get_parameter("spawn_distance_threshold").as_double(); @@ -115,6 +121,7 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options) create_subscription_options(this)); pub_goal_pose_ = create_publisher("output/goal", rclcpp::QoS(1)); + pub_marker_ = create_publisher("~/debug", 10); init_analyzer_variables(); @@ -167,6 +174,8 @@ void ReactionAnalyzerNode::on_timer() return; } + pub_marker_->publish(entity_debug_marker_); + // Spawn the obstacle if the conditions are met spawn_obstacle(current_odometry_ptr->pose.pose.position); @@ -244,7 +253,7 @@ void ReactionAnalyzerNode::calculate_results( void ReactionAnalyzerNode::init_analyzer_variables() { entity_pose_ = create_entity_pose(node_params_.entity_params); - + entity_debug_marker_ = create_polyhedron_marker(node_params_.entity_params); goal_pose_.pose = pose_params_to_pose(node_params_.goal_pose); if (node_running_mode_ == RunningMode::PlanningControl) { diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 21811430da696..8c735c42b9cd5 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -162,10 +162,14 @@ std::optional> SubscriberBase::get_m 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; } } diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index 792878e4b57fe..4a6110322440e 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -120,6 +120,86 @@ rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node) return sub_opt; } +visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "entity"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = params.x; + marker.pose.position.y = params.y; + marker.pose.position.z = params.z; + + tf2::Quaternion quaternion; + quaternion.setRPY( + tier4_autoware_utils::deg2rad(params.roll), tier4_autoware_utils::deg2rad(params.pitch), + tier4_autoware_utils::deg2rad(params.yaw)); + marker.pose.orientation = tf2::toMsg(quaternion); + + marker.scale.x = 0.1; // Line width + + marker.color.a = 1.0; // Alpha + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + // Define the 8 corners of the polyhedron + geometry_msgs::msg::Point p1, p2, p3, p4, p5, p6, p7, p8; + + p1.x = params.x_l / 2.0; + p1.y = params.y_l / 2.0; + p1.z = params.z_l / 2.0; + p2.x = -params.x_l / 2.0; + p2.y = params.y_l / 2.0; + p2.z = params.z_l / 2.0; + p3.x = -params.x_l / 2.0; + p3.y = -params.y_l / 2.0; + p3.z = params.z_l / 2.0; + p4.x = params.x_l / 2.0; + p4.y = -params.y_l / 2.0; + p4.z = params.z_l / 2.0; + p5.x = params.x_l / 2.0; + p5.y = params.y_l / 2.0; + p5.z = -params.z_l / 2.0; + p6.x = -params.x_l / 2.0; + p6.y = params.y_l / 2.0; + p6.z = -params.z_l / 2.0; + p7.x = -params.x_l / 2.0; + p7.y = -params.y_l / 2.0; + p7.z = -params.z_l / 2.0; + p8.x = params.x_l / 2.0; + p8.y = -params.y_l / 2.0; + p8.z = -params.z_l / 2.0; + + // Add points to the marker + marker.points.push_back(p1); + marker.points.push_back(p2); + marker.points.push_back(p3); + marker.points.push_back(p4); + marker.points.push_back(p1); + + marker.points.push_back(p5); + marker.points.push_back(p6); + marker.points.push_back(p7); + marker.points.push_back(p8); + marker.points.push_back(p5); + + marker.points.push_back(p1); + marker.points.push_back(p5); + marker.points.push_back(p6); + marker.points.push_back(p2); + marker.points.push_back(p3); + marker.points.push_back(p7); + marker.points.push_back(p4); + marker.points.push_back(p8); + + return marker; +} + std::vector split(const std::string & str, char delimiter) { std::vector elements; @@ -131,6 +211,11 @@ std::vector split(const std::string & str, char delimiter) return elements; } +bool does_folder_exist(const std::string & path) +{ + return std::filesystem::exists(path) && std::filesystem::is_directory(path); +} + size_t get_index_after_distance( const Trajectory & traj, const size_t curr_id, const double distance) { @@ -410,7 +495,7 @@ void write_results( } // tmp map to store latency results for statistics - std::map>> tmp_latency_map; + std::map>> tmp_latency_map; size_t test_count = 0; for (const auto & pipeline_map : pipeline_map_vector) { @@ -444,7 +529,8 @@ void write_results( const auto total_latency = calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp); file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; - tmp_latency_map[node_name].emplace_back(node_latency, total_latency); + tmp_latency_map[node_name].emplace_back( + std::make_tuple(node_latency, pipeline_latency, total_latency)); } else { const auto & prev_reaction = pipeline_reactions[j - 1].second; const auto node_latency = @@ -454,7 +540,8 @@ void write_results( const auto total_latency = calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp); file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; - tmp_latency_map[node_name].emplace_back(node_latency, total_latency); + tmp_latency_map[node_name].emplace_back( + std::make_tuple(node_latency, pipeline_latency, total_latency)); } } file << "\n"; @@ -465,27 +552,36 @@ void write_results( file << "\nStatistics\n"; file << "Node " - "Name,Min-NL,Max-NL,Mean-NL,Median-NL,Std-Dev-NL,Min-TL,Max-TL,Mean-TL,Median-TL,Std-Dev-" - "TL\n"; + "Name,Min-NL,Max-NL,Mean-NL,Median-NL,Std-Dev-NL,Min-PL,Max-PL,Mean-PL,Median-PL,Std-Dev-" + "PL,Min-TL,Max-TL,Mean-TL,Median-TL,Std-Dev-TL\n"; for (const auto & [node_name, latency_vec] : tmp_latency_map) { file << node_name << ","; + std::vector node_latencies; + std::vector pipeline_latencies; std::vector total_latencies; // Extract latencies for (const auto & latencies : latency_vec) { - node_latencies.push_back(latencies.first); - total_latencies.push_back(latencies.second); + double node_latency, pipeline_latency, total_latency; + std::tie(node_latency, pipeline_latency, total_latency) = latencies; + node_latencies.push_back(node_latency); + pipeline_latencies.push_back(pipeline_latency); + total_latencies.push_back(total_latency); } const auto stats_node_latency = calculate_statistics(node_latencies); + const auto stats_pipeline_latency = calculate_statistics(pipeline_latencies); const auto stats_total_latency = calculate_statistics(total_latencies); file << stats_node_latency.min << "," << stats_node_latency.max << "," << stats_node_latency.mean << "," << stats_node_latency.median << "," - << stats_node_latency.std_dev << "," << stats_total_latency.min << "," - << stats_total_latency.max << "," << stats_total_latency.mean << "," - << stats_total_latency.median << "," << stats_total_latency.std_dev << "\n"; + << stats_node_latency.std_dev << "," << stats_pipeline_latency.min << "," + << stats_pipeline_latency.max << "," << stats_pipeline_latency.mean << "," + << stats_pipeline_latency.median << "," << stats_pipeline_latency.std_dev << "," + << stats_total_latency.min << "," << stats_total_latency.max << "," + << stats_total_latency.mean << "," << stats_total_latency.median << "," + << stats_total_latency.std_dev << "\n"; } file.close(); RCLCPP_INFO(node->get_logger(), "Results written to: %s", ss.str().c_str());