feat(tier4_autoware_utils): add geometry utils

[kosuke55]

Description

Add geometry util functions.
This is part of #873.

Related links

#873

Tests performed

not add test code.
check pull_over module #873 can work.

Notes for reviewers

some functions are similar. maybe not needed.

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.

[tkimura4]

LGTM

This PR only moved some utility functions to tier4_autoware_utils from some packages (freespace_planning_algorithms, occulusion_spot, behavior_path_planner

This PR

[kosuke55]

This PR only moved some utility functions to tier4_autoware_utils from some packages

And added some new functions used in pull_over like inverseTransformPose etc..

[codecov]

Codecov Report

Merging #1257 (6104bef) into main (aa3ee14) will increase coverage by 0.23%.
The diff coverage is 72.22%.

@@           Coverage Diff            @@
##            main   #1257      +/-   ##
========================================
+ Coverage   9.57%   9.81%   +0.23%     
========================================
  Files       1065    1069       +4     
  Lines      73230   75749    +2519     
  Branches   16004   17402    +1398     
========================================
+ Hits        7014    7433     +419     
- Misses     60153   61766    +1613     
- Partials    6063    6550     +487     

Flag	Coverage Δ		*Carryforward flag
differential	10.25% <72.18%> (?)
total	9.56% <50.00%> (ø)		Carriedforward from 8d0da87

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

Impacted Files	Coverage Δ
...lanner/include/behavior_path_planner/utilities.hpp	0.00% <ø> (ø)
...include/scene_module/occlusion_spot/grid_utils.hpp	0.00% <ø> (ø)
...eespace_planning_algorithms/abstract_algorithm.hpp	59.09% <ø> (-5.20%)	⬇️
...autoware_utils/test/src/geometry/test_geometry.cpp	45.66% <60.00%> (+8.06%)	⬆️
...include/tier4_autoware_utils/geometry/geometry.hpp	92.00% <96.66%> (+5.33%)	⬆️
...ware_utils/test/src/trajectory/test_trajectory.cpp	32.52% <0.00%> (-1.26%)	⬇️
...ils/test/src/trajectory/test_path_with_lane_id.cpp	26.77% <0.00%> (-0.04%)	⬇️
planning/obstacle_avoidance_planner/src/node.cpp	0.00% <0.00%> (ø)
planning/obstacle_avoidance_planner/src/utils.cpp	0.00% <0.00%> (ø)
...ning/obstacle_cruise_planner/src/polygon_utils.cpp	0.00% <0.00%> (ø)
... and 27 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update aa3ee14...6104bef. Read the comment docs.

[kosuke55]

@tkimura4 @yukkysaito
I added tests for new functions in 0cf064d
I confirmed that the expected values are the same as the values calculated from another library (skrobot).

import numpy as np
from skrobot.coordinates import make_coords
from skrobot.coordinates.math import rpy2quaternion

pos = [2.0, 4.0, 6.0]
q = rpy2quaternion(np.deg2rad([30, 20, 10]))  # yaw pitch roll
p = make_coords(pos, q)

pos = [1.0, 2.0, 3.0]
q = rpy2quaternion(np.deg2rad([30, 30, 30]))
trans = make_coords(pos, q)

p_transed = trans.copy_coords().transform(p)
print(p_transed)
# <Coordinates 0x7f758df2a850 5.384 5.067 8.232 / 1.2 0.5 1.0>
print(p_transed.quaternion)  # w x y z
# [0.767046   0.24304508 0.42968035 0.4098101 ]

p_inverse_transed = trans.inverse_transformation().transform(p)
print(p_inverse_transed)
# <Coordinates 0x7f553e99c970 0.116 2.833 2.442 / 0.1 -0.2 -0.4>
print(p_inverse_transed.quaternion)  # w x y z
# [ 0.98106026 -0.17298739 -0.08189961  0.02980902]

[yukkysaito]

LGTM

[kosuke55]

test failed in humble. the values are almost equal to expected.

[kosuke55]

EXPECT_NEAR fixed test fails 6104bef, so merged.