diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md index 1719fc87e0ddf..8c8510559c678 100644 --- a/tools/reaction_analyzer/README.md +++ b/tools/reaction_analyzer/README.md @@ -76,23 +76,28 @@ start to test. After the test is completed, the results will be stored in the `o #### Perception Planning Mode - Download the rosbag files from the Google Drive - link [here](https://drive.google.com/file/d/1_P-3oy_M6eJ7fk8h5CP8V6s6da6pPrBu/view?usp=sharing). + 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`. -- Because custom sensor setup, you need to check out the following branches before launch the - reaction analyzer: For the `autoware_individual_params` repository, check out the - branch [here](https://github.com/brkay54/autoware_individual_params/tree/bk/reaction-analyzer-config). -- For the `awsim_sensor_kit_launch` repository, check out the - branch [here](https://github.com/brkay54/awsim_sensor_kit_launch/tree/bk/reaction-analyzer-config). -- After you check outed the branches, you can start to test with the following command: +- 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_sensor_kit map_path:=[MAP_PATH] +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 @@ -101,39 +106,53 @@ The parameters you need to redefine are `initialization_pose`, `entity_params`, for `perception_planning` mode) parameters.** - To set `initialization_pose`, `entity_params`, `goal_pose`: -- Upload your `.osm` map file into the [scenario editor](https://scenario.ci.tier4.jp/scenario_editor/) to define the - position of the position parameters. -- Add EGO vehicle from edit/add entity/Ego to map. -- Set destination to EGO vehicle and add another dummy object in same way. The dummy object represents the object spawn - suddenly in the reaction analyzer test. +- 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 you set up the positions in the map, we should get the positions of these entities in the map frame. To achieve -this:** +- 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. -- Convert the positions to map frame by changing Map/Coordinate to World and Map/Orientation to Euler in Scenario - Editor. +**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 these steps, you can see the positions in map frame and euler angles. You can change - the `initialization_pose`, `entity_params`, `goal_pose` parameters with the values you get from the website. +- 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 +``` -**For the `topic_publisher` parameters, you need to record the rosbags from the AWSIM. After opened your AWSIM -environment, you should record two different rosbags. However, the environment should be static and the position of the -vehicle should be same.** +- 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. -- Record a rosbag in empty environment (without an obstacle in front of the vehicle). -- After that, record another rosbag in the same environment except add an object in front of the vehicle. +**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.** +- After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the + paths of the recorded rosbags. ## Results The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each -pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, and `Total Latency` +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. -- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent timestamp. +- `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 diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 7de1b06a145b3..631233cef8a7d 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -100,7 +100,7 @@ class ReactionAnalyzerNode : public rclcpp::Node std::optional test_environment_init_time_; std::optional spawn_cmd_time_; std::atomic spawn_object_cmd_{false}; - bool is_vehicle_initialized_{false}; + bool is_initialization_requested{false}; bool is_route_set_{false}; size_t test_iteration_count_{0}; diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp index 6fe1e725892a1..e2e9ea072970e 100644 --- a/tools/reaction_analyzer/include/topic_publisher.hpp +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -223,13 +223,12 @@ class TopicPublisher 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); - std::map> set_general_publishers_and_timers(); - void set_pointcloud_publishers_and_timers( + 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_publishers_with_object(const std::string & path_bag_with_object); - void init_rosbag_publishers_without_object(const std::string & path_bag_without_object); + 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< diff --git a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml index 1b0aed0c2a92f..b0b1d4fb9bf2e 100644 --- a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml +++ b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml @@ -1,7 +1,7 @@ - + @@ -23,7 +23,6 @@ - @@ -51,6 +50,7 @@ + @@ -61,7 +61,6 @@ - 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/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index ef90428696dd8..62c1191bd345a 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -2,37 +2,37 @@ ros__parameters: timer_period: 0.033 # s test_iteration: 10 - output_file_path: + 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: 81247.9609375 - y: 49828.87890625 + x: 81546.984375 + y: 50011.96875 z: 0.0 roll: 0.0 pitch: 0.0 - yaw: 35.5 + yaw: 11.1130405 goal_pose: - x: 81824.90625 - y: 50069.34375 + x: 81643.0703125 + y: 50029.8828125 z: 0.0 roll: 0.0 pitch: 0.0 - yaw: 8.9305903 + yaw: 13.12 entity_params: - x: 81392.97671487389 - y: 49927.361443601316 - z: 42.13605499267578 - roll: 0.2731273 - pitch: -0.6873504 - yaw: 33.7664809 - x_dimension: 4.118675972722859 - y_dimension: 1.7809072588403219 - z_dimension: 0.8328610206872963 + 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: /lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3 - path_bag_with_object: /lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3 + 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 diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp index db1d539e14e7b..e7d01ecef6842 100644 --- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -269,11 +269,14 @@ void ReactionAnalyzerNode::init_test_env( last_test_environment_init_request_time_ = current_time; // Pose initialization of the ego - is_vehicle_initialized_ = !is_vehicle_initialized_ - ? init_ego(initialization_state_ptr, odometry_ptr, current_time) - : is_vehicle_initialized_; + is_initialization_requested = !is_initialization_requested + ? init_ego(initialization_state_ptr, odometry_ptr, current_time) + : is_initialization_requested; - if (!is_vehicle_initialized_) { + if ( + is_initialization_requested && + initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { + is_initialization_requested = false; return; } @@ -298,7 +301,7 @@ void ReactionAnalyzerNode::init_test_env( } const bool is_ready = - (is_vehicle_initialized_ && is_route_set_ && + (is_initialization_requested && is_route_set_ && (operation_mode_ptr->mode == OperationModeState::AUTONOMOUS || node_running_mode_ == RunningMode::PerceptionPlanning)); if (is_ready) { @@ -332,14 +335,12 @@ bool ReactionAnalyzerNode::init_ego( { // Pose initialization of the ego if (initialization_state_ptr) { - if (initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { - 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_); - } - return false; + 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) { @@ -407,7 +408,7 @@ void ReactionAnalyzerNode::reset() return; } // reset the variables - is_vehicle_initialized_ = false; + is_initialization_requested = false; is_route_set_ = false; test_environment_init_time_ = std::nullopt; last_test_environment_init_request_time_ = std::nullopt; diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp index 31499de563f4b..9816f1970debd 100644 --- a/tools/reaction_analyzer/src/topic_publisher.cpp +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -251,10 +251,10 @@ void TopicPublisher::reset() void TopicPublisher::init_rosbag_publishers() { // read messages without object - init_rosbag_publishers_without_object(topic_publisher_params_.path_bag_without_object); + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_without_object, true); // read messages with object - init_rosbag_publishers_with_object(topic_publisher_params_.path_bag_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. @@ -264,10 +264,7 @@ void TopicPublisher::init_rosbag_publishers() RCLCPP_ERROR(node_->get_logger(), "Messages are not initialized correctly"); rclcpp::shutdown(); } - - // set publishers and timers except for the pointcloud message - const auto pointcloud_variables_map = set_general_publishers_and_timers(); // except pointcloud - set_pointcloud_publishers_and_timers(pointcloud_variables_map); + set_publishers_and_timers_to_variable(); } template @@ -331,8 +328,7 @@ void TopicPublisher::set_period(const std::map> -TopicPublisher::set_general_publishers_and_timers() +void TopicPublisher::set_publishers_and_timers_to_variable() { std::map> pointcloud_variables_map; // temp map for pointcloud publishers @@ -349,11 +345,10 @@ TopicPublisher::set_general_publishers_and_timers() [&](auto & var) { using MessageType = typename decltype(var.empty_area_message)::element_type; - // Check if the MessageType is PointCloud2 if constexpr ( std::is_same_v || - std::is_same_v) { - // For PointCloud2, use rclcpp::SensorDataQoS + 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 @@ -379,10 +374,12 @@ TopicPublisher::set_general_publishers_and_timers() variant); } - return pointcloud_variables_map; + // 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_pointcloud_publishers_and_timers( +void TopicPublisher::set_timers_for_pointcloud_msgs( const std::map> & pointcloud_variables_map) { // Set the point cloud publisher timers @@ -470,12 +467,13 @@ bool TopicPublisher::check_publishers_initialized_correctly() return true; } -void TopicPublisher::init_rosbag_publishers_with_object(const std::string & path_bag_with_object) +void TopicPublisher::init_rosbag_publisher_buffer( + const std::string & bag_path, const bool is_empty_area_message) { rosbag2_cpp::Reader reader; try { - reader.open(path_bag_with_object); + reader.open(bag_path); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Error opening bag file: " << e.what()); rclcpp::shutdown(); @@ -483,7 +481,6 @@ void TopicPublisher::init_rosbag_publishers_with_object(const std::string & path } const auto & topics = reader.get_metadata().topics_with_message_count; - const bool is_empty_area_message = false; while (reader.has_next()) { auto bag_message = reader.read_next(); @@ -532,80 +529,4 @@ void TopicPublisher::init_rosbag_publishers_with_object(const std::string & path } reader.close(); } - -void TopicPublisher::init_rosbag_publishers_without_object( - const std::string & path_bag_without_object) -{ - rosbag2_cpp::Reader reader; - - try { - reader.open(path_bag_without_object); - } 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; - const bool is_empty_area_message = true; - - // Collect timestamps for each topic to set the frequency of the publishers - std::map> timestamps_per_topic; - - while (reader.has_next()) { - const 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; - } - - // Record timestamp - timestamps_per_topic[current_topic].emplace_back(bag_message->time_stamp); - - // Deserialize and store the first message as a sample - 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); - } - } - - // After collecting all timestamps for each topic, set frequencies of the publishers - set_period(timestamps_per_topic); - - reader.close(); -} } // namespace reaction_analyzer::topic_publisher diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index e6f2156762dd8..792878e4b57fe 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -416,26 +416,22 @@ void write_results( for (const auto & pipeline_map : pipeline_map_vector) { test_count++; // convert pipeline_map to vector of tuples - const auto sorted_results_vector = convert_pipeline_map_to_sorted_vector(pipeline_map); 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()); - rclcpp::Time spawn_cmd_time; // init it to parse total latency for (size_t i = 0; i < sorted_results_vector.size(); ++i) { const auto & [pipeline_header_time, pipeline_reactions] = sorted_results_vector[i]; - if (i == 0) { - spawn_cmd_time = pipeline_reactions[0].second.header.stamp; - } - // total time pipeline lasts - file << "Pipeline - " << i + 1 << ","; + file << "Pipeline - " << i << ","; // pipeline nodes for (const auto & [node_name, reaction] : pipeline_reactions) { file << node_name << ","; } - file << "\nNode Latency - Total Latency [ms],"; + file << "\nNode - Pipeline - Total Latency [ms],"; for (size_t j = 0; j < pipeline_reactions.size(); ++j) { const auto & reaction = pipeline_reactions[j].second; @@ -443,17 +439,21 @@ void write_results( 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 << " - " << total_latency << ","; + file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; tmp_latency_map[node_name].emplace_back(node_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 << " - " << total_latency << ","; + file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; tmp_latency_map[node_name].emplace_back(node_latency, total_latency); } }