feat(reaction_analyzer): add reaction anaylzer tool to measure end-to-end delay in sudden obstacle braking response

[brkay54]

Description

Main issue:

• Add a new tool to measure end-to-end delay of the Autoware for sudden obstacles #6547

Strictly depends on:

• feat(tier4_simulator_launch): add option to disable all perception related modules #6382

Optionally depends on:

• Make it possible to detect publishing times of the messages in the pipelines #6255

The main purpose of the reaction analyzer package is to measure the reaction times of the nodes by listening to the
pre-determined topics. It can be used for measuring the reaction time of the perception pipeline, planning pipeline, and
control pipeline. To be able to measure both control outputs and perception outputs, it is necessary to divide the node
into two running_mode: planning_control and perception_planning.

Planning Control Mode

In this mode, the reaction analyzer creates a dummy publisher for the PredictedObjects and PointCloud 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
measure the pre-determined topics to catch the reacted messages of the planning and control nodes. When all the topics are reacted, it calculates the reaction time of the nodes and statistics, 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 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 measure the pre-determined
topics to catch the reacted messages of the perception and planning nodes. When all the topics are reacted, it
calculates
the reaction time of the nodes and statistics, and it creates a CSV file to store the results.

Related links

Related Issue:

• Investigate end-to-end delay in sudden obstacle braking response #5540

Tests performed

You can see the results in the Excel table here. You can see the test dates below the page.
Also, some tests were shared in issues comments: #5540

Notes for reviewers

This PR currently depends on other PR below:

• feat(tier4_simulator_launch): add option to disable all perception related modules #6382

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[brkay54]

@mitsudome-r -san, actually I saw this issue before but I didn't focus on that issue because it does not prevent running reaction_analyzer in planning control mode. Probably the error you encounter is similar.

Here log of the error:

[component_container_mt-1] [WARN 1715586401.197737367] [probabilistic_occupancy_grid_map]: Failed to lookup transform: Lookup would require extrapolation into the future.  Requested time 1715586400.691542 but the latest data is at time 1715586400.655299, when looking up transform from frame [base_link] to frame [map] (transformPointcloud() at /home/berkay/projects/autoware/src/universe/autoware.universe/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp:35)
[component_container_mt-1] terminate called after throwing an instance of 'std::runtime_error'
[component_container_mt-1]   what():  Field x does not exist
[component_container_mt-1] *** Aborted at 1715586401 (unix time) try "date -d @1715586401" if you are using GNU date ***
[component_container_mt-1] PC: @                0x0 (unknown)
[component_container_mt-1] *** SIGABRT (@0x3e8001bf6b6) received by PID 1832630 (TID 0x7f54e1ffb640) from PID 1832630; stack trace: ***
[component_container_mt-1]     @     0x7f550c40a006 google::(anonymous namespace)::FailureSignalHandler()
[component_container_mt-1]     @     0x7f5510842520 (unknown)
[component_container_mt-1]     @     0x7f55108969fc pthread_kill
[component_container_mt-1]     @     0x7f5510842476 raise
[component_container_mt-1]     @     0x7f55108287f3 abort
[component_container_mt-1]     @     0x7f5510ca2b9e (unknown)
[component_container_mt-1]     @     0x7f5510cae20c (unknown)
[component_container_mt-1]     @     0x7f5510cae277 std::terminate()
[component_container_mt-1]     @     0x7f5510cae4d8 __cxa_throw
[component_container_mt-1]     @     0x7f550470745b sensor_msgs::impl::PointCloud2IteratorBase<>::set_field()
[component_container_mt-1]     @     0x7f550470682b costmap_2d::OccupancyGridMap::raytraceFreespace()
[component_container_mt-1]     @     0x7f5504706d0b costmap_2d::OccupancyGridMap::raytrace2D()
[component_container_mt-1]     @     0x7f55046c19a6 occupancy_grid_map::LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw()
[component_container_mt-1]     @     0x7f55046ff00b message_filters::CallbackHelper9T<>::call()
[component_container_mt-1]     @     0x7f5504700da0 message_filters::sync_policies::ExactTime<>::checkTuple()
[component_container_mt-1]     @     0x7f550470267d message_filters::Synchronizer<>::cb<>()
[component_container_mt-1]     @     0x7f55046fdea8 message_filters::CallbackHelper1T<>::call()
[component_container_mt-1]     @     0x7f55046fe007 _ZNSt17_Function_handlerIFvSt10shared_ptrIKN11sensor_msgs3msg10LaserScan_ISaIvEEEEEZN15message_filters10SubscriberIS5_N6rclcpp4NodeEE9subscribeEPSC_RKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE17rmw_qos_profile_sNSB_32SubscriptionOptionsWithAllocatorIS4_EEEUlS7_E_E9_M_invokeERKSt9_Any_dataOS7_
[component_container_mt-1]     @     0x7f55046d6752 _ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN11sensor_msgs3msg10LaserScan_ISaIvEEESA_E8dispatchESt10shared_ptrISB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRKSB_EESN_IFvSP_SH_EESN_IFvRKNS5_17SerializedMessageEEESN_IFvSW_SH_EESN_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESN_IFvS14_SH_EESN_IFvS11_ISU_S12_ISU_EEEESN_IFvS1A_SH_EESN_IFvSD_ISO_EEESN_IFvS1F_SH_EESN_IFvSD_ISV_EEESN_IFvS1K_SH_EESN_IFvRKS1F_EESN_IFvS1Q_SH_EESN_IFvRKS1K_EESN_IFvS1W_SH_EESN_IFvSE_EESN_IFvSE_SH_EESN_IFvSD_ISU_EEESN_IFvS25_SH_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESL_S2B_
[component_container_mt-1]     @     0x7f55046fb3d3 rclcpp::Subscription<>::handle_message()
[component_container_mt-1]     @     0x7f55110237bc rclcpp::Executor::execute_subscription()
[component_container_mt-1]     @     0x7f5511023fbf rclcpp::Executor::execute_any_executable()
[component_container_mt-1]     @     0x7f551102b27a rclcpp::executors::MultiThreadedExecutor::run()
[component_container_mt-1]     @     0x7f5510cdc253 (unknown)
[component_container_mt-1]     @     0x7f5510894ac3 (unknown)
[component_container_mt-1]     @     0x7f5510926850 (unknown)
[ERROR] [component_container_mt-1]: process has died [pid 1832630, exit code -6, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=pointcloud_container -r __ns:=/ -p use_sim_time:=False -p wheel_radius:=0.383 -p wheel_width:=0.235 -p wheel_base:=2.79 -p wheel_tread:=1.64 -p front_overhang:=1.0 -p rear_overhang:=1.1 -p left_overhang:=0.128 -p right_overhang:=0.128 -p vehicle_height:=2.5 -p max_steer_angle:=0.7'].

Let's discuss this at tomorrow's meeting.

[brkay54]

@mitsudome-r -san, I guess, I found the problem. When we start to publish the empty pointcloud before the initialization, it causes the crash above. We should publish the pointcloud message after the vehicle is initialized. I tried 7 times in planning_control mode and I didn't get the error we mentioned above. When you have time, could you test it again with the latest commit?

[beyzanurkaya]

Very nice work @brkay54
I tried this PR on my own computer and I think a few minor improvements can be made.

• Firstly, when running the planning_control mode without making any changes to the param file, including file paths, I noticed that even with invalid file paths in the param file, the node worked without giving any error or warning. You can throw an error when an invalid file path is given to make sure the user can see the results after testing.

• Secondly, in the scenario created for the planning_control mode, the vehicle starts driving and stops for an object close to the goal point, abruptly ending the test. However, since the object is far from the ego's starting point, it may not be immediately clear whether there is an object at that point until the vehicle moves forward for a while. This could lead to confusion about why the test ends at that specific point. To improve clarity, you can add a marker symbolizing the object there or start the next test a few seconds after the vehicle has stopped completely.

• When I ran the perception_planning mode (with no changes made to the param file other than the bag and output file paths), I encountered an issue where the test did not complete. Although the vehicle was initialized, the goal was set, and the route was generated with objects properly detected, the test did not proceed beyond this point. No output file was created. However, when I commented out the control topics in the yaml file and retried, the tests were completed successfully, and the result file was created. In this regard, it would be useful for users to specify the reaction topics that the mods expect separately or to add a warning about this to the readme.

Also, I did not encounter any node crashes like in the last comment. I think the README file was quite descriptive. Overall, it's a well-done piece of work!

[brkay54]

@beyzanurkaya Thank you for the review.

Firstly, when running the planning_control mode without making any changes to the param file, including file paths, I noticed that even with invalid file paths in the param file, the node worked without giving any error or warning. You can throw an error when an invalid file path is given to make sure the user can see the results after testing.

I have added a condition for checking whether the output path is valid or not at the beginning of the node.

Secondly, in the scenario created for the planning_control mode, the vehicle starts driving and stops for an object close to the goal point, abruptly ending the test. However, since the object is far from the ego's starting point, it may not be immediately clear whether there is an object at that point until the vehicle moves forward for a while. This could lead to confusion about why the test ends at that specific point. To improve clarity, you can add a marker symbolizing the object there or start the next test a few seconds after the vehicle has stopped completely.

Thank you for the suggestion. I added a debug marker for the entity that will be spawned.

When I ran the perception_planning mode (with no changes made to the param file other than the bag and output file paths), I encountered an issue where the test did not complete. Although the vehicle was initialized, the goal was set, and the route was generated with objects properly detected, the test did not proceed beyond this point. No output file was created. However, when I commented out the control topics in the yaml file and retried, the tests were completed successfully, and the result file was created. In this regard, it would be useful for users to specify the reaction topics that the mods expect separately or to add a warning about this to the readme.

Because the vehicle is not moving in perception_planning mode, the control topics may not react. Thus the reaction_analyzer waits for them to react. I added this warning in the readme, I also added a warning message prints reaction_analyzer is waiting for which node.

I appreciate your review. Could you also try with the last commit?

[mitsudome-r]

LGTM

It was old review.

[beyzanurkaya]

LGTM