Fix/canonicalize function

[hakuturu583]

Description

Abstract

catch common::SemanticError inside traffic_simulator::pose::canonicalize function.

Background

In #1266 the CanonicalizedLaneletPose::canonicalize function is now used in the constructor of the CanonicalizedLaneletPose class.
CanonicalizedLaneletPose class is used inside traffic_simulator::pose::canonicalize function.
The traffic_simulator::pose::canonicalize function has a return value of type std::optional and must return std::nullopt if it is used for lane coordinate system calculation, but CanonicalizedLaneletPose ::canonicalize function throws common::SemanticError in case of failure of lane coordinate system calculation, so if std::nullopt is expected as a return value, an exception is raised and the simulation stops.

Details

catch common::SemanticError inside traffic_simulator::pose::canonicalize function.

Unit tests run on the source code of this commit hash will output the following error and fail.

- test_pose
  <<< failure message
    -- run_test.py: invoking following command in '/home/user/workspace/pilot-auto.x2/build/traffic_simulator/test/src/utils':
     - /home/user/workspace/pilot-auto.x2/build/traffic_simulator/test/src/utils/test_pose --gtest_output=xml:/home/user/workspace/pilot-auto.x2/build/traffic_simulator/test_results/traffic_simulator/test_pose.gtest.xml
    [==========] Running 44 tests from 2 test suites.
    [----------] Global test environment set-up.
    [----------] 6 tests from pose
    [ RUN      ] pose.quietNaNPose
    [       OK ] pose.quietNaNPose (0 ms)
    [ RUN      ] pose.quietNaNLaneletPose
    [       OK ] pose.quietNaNLaneletPose (0 ms)
    [ RUN      ] pose.transformRelativePoseToGlobal
    [       OK ] pose.transformRelativePoseToGlobal (0 ms)
    [ RUN      ] pose.relativePose_poses_zero
    [       OK ] pose.relativePose_poses_zero (0 ms)
    [ RUN      ] pose.relativePose_poses_zeros
    [       OK ] pose.relativePose_poses_zeros (0 ms)
    [ RUN      ] pose.relativePose_poses_complex
    [       OK ] pose.relativePose_poses_complex (0 ms)
    [----------] 6 tests from pose (0 ms total)
    
    [----------] 38 tests from PoseTest
    [ RUN      ] PoseTest.canonicalize_default
    [       OK ] PoseTest.canonicalize_default (8 ms)
    [ RUN      ] PoseTest.canonicalize_invalid
    unknown file: Failure
    C++ exception with description "/home/user/workspace/pilot-auto.x2/src/simulator/scenario_simulator/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp:81: Lanelet pose (id=203,s=1000,offset=0,rpy.x=0,rpy.y=0,rpy.z=0) is invalid, please check lanelet length and connection." thrown in the test body.
    [  FAILED  ] PoseTest.canonicalize_invalid (7 ms)
    [ RUN      ] PoseTest.canonicalize_valid
    [       OK ] PoseTest.canonicalize_valid (7 ms)
    [ RUN      ] PoseTest.toMapPose_CanonicalizedLaneletPose
    [       OK ] PoseTest.toMapPose_CanonicalizedLaneletPose (6 ms)
    [ RUN      ] PoseTest.toMapPose_LaneletPose
    [       OK ] PoseTest.toMapPose_LaneletPose (7 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid (7 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid (6 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_noRoute_valid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_noRoute_valid (7 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid (7 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid (7 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_emptyValid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_emptyValid (6 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid (7 ms)
    [ RUN      ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid
    [       OK ] PoseTest.toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid (7 ms)
    [ RUN      ] PoseTest.relativePose_canonicalized_end_position
    [       OK ] PoseTest.relativePose_canonicalized_end_position (6 ms)
    [ RUN      ] PoseTest.relativePose_canonicalized_start_position
    [       OK ] PoseTest.relativePose_canonicalized_start_position (7 ms)
    [ RUN      ] PoseTest.boundingBoxRelativePose_disjoint
    [       OK ] PoseTest.boundingBoxRelativePose_disjoint (6 ms)
    [ RUN      ] PoseTest.boundingBoxRelativePose_commonEdge
    [       OK ] PoseTest.boundingBoxRelativePose_commonEdge (7 ms)
    [ RUN      ] PoseTest.boundingBoxRelativePose_intersect
    [       OK ] PoseTest.boundingBoxRelativePose_intersect (7 ms)
    [ RUN      ] PoseTest.relativeLaneletPose_s_invalid
    [       OK ] PoseTest.relativeLaneletPose_s_invalid (7 ms)
    [ RUN      ] PoseTest.relativeLaneletPose_s_valid
    [       OK ] PoseTest.relativeLaneletPose_s_valid (7 ms)
    [ RUN      ] PoseTest.relativeLaneletPose_offset_invalid
    [       OK ] PoseTest.relativeLaneletPose_offset_invalid (6 ms)
    [ RUN      ] PoseTest.relativeLaneletPose_offset_valid
    [       OK ] PoseTest.relativeLaneletPose_offset_valid (7 ms)
    [ RUN      ] PoseTest.boundingBoxRelativeLaneletPose_s_invalid
    [       OK ] PoseTest.boundingBoxRelativeLaneletPose_s_invalid (7 ms)
    [ RUN      ] PoseTest.boundingBoxRelativeLaneletPose_s_valid
    [       OK ] PoseTest.boundingBoxRelativeLaneletPose_s_valid (7 ms)
    [ RUN      ] PoseTest.boundingBoxRelativeLaneletPose_offset_invalid
    [       OK ] PoseTest.boundingBoxRelativeLaneletPose_offset_invalid (7 ms)
    [ RUN      ] PoseTest.boundingBoxRelativeLaneletPose_offset_valid
    [       OK ] PoseTest.boundingBoxRelativeLaneletPose_offset_valid (6 ms)
    [ RUN      ] PoseTest.isInLanelet_inside
    [       OK ] PoseTest.isInLanelet_inside (8 ms)
    [ RUN      ] PoseTest.isInLanelet_outsideFrontFar
    [       OK ] PoseTest.isInLanelet_outsideFrontFar (7 ms)
    [ RUN      ] PoseTest.isInLanelet_outsideFrontClose
    [       OK ] PoseTest.isInLanelet_outsideFrontClose (6 ms)
    [ RUN      ] PoseTest.isInLanelet_outsideBackFar
    [       OK ] PoseTest.isInLanelet_outsideBackFar (8 ms)
    [ RUN      ] PoseTest.isInLanelet_outsideBackClose
    [       OK ] PoseTest.isInLanelet_outsideBackClose (7 ms)
    [ RUN      ] PoseTest.isAtEndOfLanelets_noFollowing_within
    [       OK ] PoseTest.isAtEndOfLanelets_noFollowing_within (7 ms)
    [ RUN      ] PoseTest.isAtEndOfLanelets_singleFollowing_within
    [       OK ] PoseTest.isAtEndOfLanelets_singleFollowing_within (6 ms)
    [ RUN      ] PoseTest.isAtEndOfLanelets_singleFollowing_outside
    [       OK ] PoseTest.isAtEndOfLanelets_singleFollowing_outside (7 ms)
    [ RUN      ] PoseTest.isAtEndOfLanelets_multipleFollowing_within
    [       OK ] PoseTest.isAtEndOfLanelets_multipleFollowing_within (7 ms)
    [ RUN      ] PoseTest.isAtEndOfLanelets_multipleFollowing_outside
    [       OK ] PoseTest.isAtEndOfLanelets_multipleFollowing_outside (7 ms)
    [ RUN      ] PoseTest.laneletLength_invalid
    [       OK ] PoseTest.laneletLength_invalid (7 ms)
    [ RUN      ] PoseTest.laneletLength_valid
    [       OK ] PoseTest.laneletLength_valid (7 ms)
    [----------] 38 tests from PoseTest (260 ms total)
    
    [----------] Global test environment tear-down
    [==========] 44 tests from 2 test suites ran. (260 ms total)
    [  PASSED  ] 43 tests.
    [  FAILED  ] 1 test, listed below:
    [  FAILED  ] PoseTest.canonicalize_invalid

References

Regression test result.

Destructive Changes

N/A

Known Limitations

N/A

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[sonarqubecloud]

Quality Gate passed

Issues
0 New issues
0 Accepted issues

Measures
0 Security Hotspots
83.3% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud