fix(image_projection_based_fusion): handle projection errors in image fusion nodes

[technolojin]

Description

The ROS2 PinholeCameraModel::unrectifyPoint can cause exception

http://docs.ros.org/en/noetic/api/image_geometry/html/c++/pinhole__camera__model_8cpp_source.html#l00423

[component_container_mt-1] terminate called after throwing an instance of 'image_geometry::Exception'
[component_container_mt-1]   what():  Cannot call unrectifyPoint when distortion is unknown.
[component_container_mt-1] *** Aborted at 1719556831 (unix time) try "date -d @1719556831" if you are using GNU date ***
[component_container_mt-1] [WARN] [1719556831.286755823] [sensing.lidar.concatenate_data]: Empty sensor points!
[component_container_mt-1] PC: @                0x0 (unknown)
[component_container_mt-1] *** SIGABRT (@0x3e80001cfd4) received by PID 118740 (TID 0x7f5a757fa640) from PID 118740; stack trace: ***
[component_container_mt-1]     @     0x7f5aa4f044d6 google::(anonymous namespace)::FailureSignalHandler()
[component_container_mt-1]     @     0x7f5aa47af520 (unknown)
[component_container_mt-1]     @     0x7f5aa48039fc pthread_kill
[component_container_mt-1]     @     0x7f5aa47af476 raise
[component_container_mt-1]     @     0x7f5aa47957f3 abort
[component_container_mt-1]     @     0x7f5aa4a58b9e (unknown)
[component_container_mt-1]     @     0x7f5aa4a6420c (unknown)
[component_container_mt-1]     @     0x7f5aa4a631e9 (unknown)
[component_container_mt-1]     @     0x7f5aa4a63959 __gxx_personality_v0
[component_container_mt-1]     @     0x7f5aa49ac884 (unknown)
[component_container_mt-1]     @     0x7f5aa49acf41 _Unwind_RaiseException
[component_container_mt-1]     @     0x7f5aa4a644cb __cxa_throw
[component_container_mt-1]     @     0x7f596c3adc45 (unknown)
[component_container_mt-1]     @     0x7f596c3aff90 image_geometry::PinholeCameraModel::unrectifyPoint()
[component_container_mt-1]     @     0x7f596c5c6b1e image_projection_based_fusion::calcRawImageProjectedPoint()
[component_container_mt-1]     @     0x7f596c6afbc0 _ZN29image_projection_based_fusion23PointPaintingFusionNode17fuseOnSingleImageERKN11sensor_msgs3msg12PointCloud2_ISaIvEEEmRKN21tier4_perception_msgs3msg27DetectedObjectsWithFeature_IS4_EERKNS2_11CameraInfo_IS4_EERS5_._omp_fn.0
[component_container_mt-1]     @     0x7f5a2e72da16 GOMP_parallel
[component_container_mt-1]     @     0x7f596c6b253b image_projection_based_fusion::PointPaintingFusionNode::fuseOnSingleImage()
[component_container_mt-1]     @     0x7f596c5b8659 image_projection_based_fusion::FusionNode<>::subCallback()
[component_container_mt-1]     @     0x7f596c6c82e8 std::_Function_handler<>::_M_invoke()
[component_container_mt-1]     @     0x7f5a705b0567 _ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN11sensor_msgs3msg12PointCloud2_ISaIvEEESA_E22dispatch_intra_processESt10shared_ptrIKSB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRSE_EESO_IFvSP_SI_EESO_IFvRKNS5_17SerializedMessageEEESO_IFvSW_SI_EESO_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESO_IFvS14_SI_EESO_IFvS11_ISU_S12_ISU_EEEESO_IFvS1A_SI_EESO_IFvSF_EESO_IFvSF_SI_EESO_IFvSD_ISV_EEESO_IFvS1J_SI_EESO_IFvRKSF_EESO_IFvS1P_SI_EESO_IFvRKS1J_EESO_IFvS1V_SI_EESO_IFvSD_ISB_EEESO_IFvS20_SI_EESO_IFvSD_ISU_EEESO_IFvS25_SI_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESM_S2B_
[component_container_mt-1]     @     0x7f596c6dd755 rclcpp::experimental::SubscriptionIntraProcess<>::execute_impl<>()
[component_container_mt-1]     @     0x7f5aa4d822c3 rclcpp::Executor::execute_any_executable()
[component_container_mt-1]     @     0x7f5aa4d88432 rclcpp::executors::MultiThreadedExecutor::run()
[component_container_mt-1]     @     0x7f5aa4a92253 (unknown)
[component_container_mt-1]     @     0x7f5aa4801ac3 (unknown)
[component_container_mt-1]     @     0x7f5aa4893850 (unknown)

To prevent the node stop, the camera info is checked before. If distortion_state can be 'UNKNOWN', stop the process.

Reference: http://docs.ros.org/en/diamondback/api/image_geometry/html/c++/pinhole__camera__model_8cpp_source.html#l00110

Test

1. Tested by fault injection.

  sensor_msgs::msg::CameraInfo camera_info = camera_info_in;
  camera_info.distortion_model = "hoge";

2. TIER IV Cloud Tester

Related links

#7053

TIER IV INTERNAL

Data:TIER IV INTERNAL

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[yukkysaito]

https://github.com/autowarefoundation/autoware.universe/pull/7030/files#r1602399641
A little more discussion may be needed on this one.
Because Exception is right in a way, it may be necessary to destroy the root cause.

[technolojin]

@kminoda
https://github.com/autowarefoundation/autoware.universe/pull/7030/files#r1604240696

we should avoid catching this error and just logging warning without killing the node

I disagree with this statement since the ROS is not deterministic. The input cannot be aligned as intended, and therefore, the module need to treat incomplete input possibilities.

[kminoda]

@technolojin As discussed previously, since we are still not sure the exact cause of this error, I propose to make this PR a draft or closed, and re-open once we isolate the real cause.

[knzo25]

@kminoda
Sorry for joining this conversation all of a sudden. I was not aware of this issue.
Using a try inside a heavy for loop, even worse, one with pragma, is not a good idea.
If we come a cross some data to reproduce it I can fix this, otherwise we can always not rely on on opencv and do the math ourselves (and in the gpu)

[technolojin]

Autoware Evaluator test has been passed TIER IV Cloud Tester

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 11 lines in your changes missing coverage. Please review.

Project coverage is 23.94%. Comparing base (e9fe665) to head (7d28fed).
Report is 2 commits behind head on main.

Files	Patch %	Lines
..._image_projection_based_fusion/src/utils/utils.cpp	0.00%	7 Missing ⚠️
...ction_based_fusion/src/roi_cluster_fusion/node.cpp	0.00%	1 Missing ⚠️
...sed_fusion/src/roi_detected_object_fusion/node.cpp	0.00%	1 Missing ⚠️
...on_based_fusion/src/roi_pointcloud_fusion/node.cpp	0.00%	1 Missing ⚠️
...fusion/src/segmentation_pointcloud_fusion/node.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #7747      +/-   ##
==========================================
+ Coverage   23.90%   23.94%   +0.03%     
==========================================
  Files        1382     1381       -1     
  Lines      101915   101560     -355     
  Branches    38808    38614     -194     
==========================================
- Hits        24365    24315      -50     
+ Misses      75050    74740     -310     
- Partials     2500     2505       +5     

Flag	Coverage Δ		*Carryforward flag
differential	4.25% <0.00%> (?)
total	23.94% <ø> (+0.03%)	⬆️	Carriedforward from b96c902

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[kminoda]

@knzo25 Sorry for missing the mention. I think the current validation scheme in this PR is OK, right? (Taekjin-san has fixed it)

[technolojin]

@yukkysaito Can you review this PR?

[kminoda]

LGTM