feat(tier4_autoware_utils): expand geometry and trajectory utility functions

[satoshi-ota]

Description

In this PR, I expanded geometry and trajectory utility functions:

geometry

support autoware_auto_planning_msgs::msg::PathWithLaneId

template <>
inline geometry_msgs::msg::Point getPoint(
  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p);

template <>
inline geometry_msgs::msg::Pose getPose(
  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p);

add new utility functions

• setPose()

template <class T>
void setPose(const geometry_msgs::msg::Pose & pose, [[maybe_unused]] T & p);

template <>
inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & p);

template <>
inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::PoseStamped & p);

template <>
inline void setPose(
  const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p);

template <>
inline void setPose(
  const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p);

template <>
inline void setPose(
  const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p);

trajectory

add new utility functions

• calcLongitudinalOffsetPoint()

/**
 * @brief calculate the point offset from source point along the trajectory (or path)
 * @param points points of trajectory, path, ...
 * @param src_idx index of source point
 * @param offset length of offset from source point
 * @return offset point
 */
template <class T>
boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPoint(
  const T & points, const size_t src_idx, const double offset);

/**
 * @brief calculate the point offset from source point along the trajectory (or path)
 * @param points points of trajectory, path, ...
 * @param src_point source point
 * @param offset length of offset from source point
 * @return offset point
 */
template <class T>
boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPoint(
  const T & points, const geometry_msgs::msg::Point & src_point, const double offset);

• calcLongitudinalOffsetPose()

/**
 * @brief calculate the point offset from source point along the trajectory (or path)
 * @param points points of trajectory, path, ...
 * @param src_idx index of source point
 * @param offset length of offset from source point
 * @return offset pose
 */
template <class T>
boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPose(
  const T & points, const size_t src_idx, const double offset);

/**
 * @brief calculate the point offset from source point along the trajectory (or path)
 * @param points points of trajectory, path, ...
 * @param src_point source point
 * @param offset length of offset from source point
 * @return offset pose
 */
template <class T>
boost::optional<geometry_msgs::msg::Point> calcLongitudinalOffsetPoint(
  const T & points, const geometry_msgs::msg::Point & src_point, const double offset);

• insertTargetPoint()

/**
 * @brief calculate the point offset from source point along the trajectory (or path)
 * @param seg_idx segment index of point at beginning of length
 * @param p_target point to be inserted
 * @param points output points of trajectory, path, ...
 * @return index of insert point
 */
template <class T>
inline size_t insertTargetPoint(
  const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points,
  const double overlap_threshold = 1e-3)

Related links

• Add utility functions to tier4_autoware_utils that inserts target velocity into a path or trajectory #596

Tests performed

• please check whether unit test is valid.

Notes for reviewers

Example usage

Insert a stop point 5.0m before the obstacle.

...
auto output_trajectory = input_trajectory;

const auto obstacle_poisition = ... (geometry_msgs::msg::Point)
const auto target_point = calcLongitudinalOffsetPoint(input_trajectory.points, obstacle_position, -5.0);
const size_t base_idx = findNearestSegmetnIndex(input_trajectory.points, obstacle_position);
const size_t insert_idx = insertTargetPoint(base_idx, target_point, output_trajectory.points);

for (size_t i = insert_idx; i < output_trajectory.points.size(); ++i)
{
  output_trajectory.points.at(i).longitudinal_velocity_mps = 0.0;
}
...

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.

[codecov]

Codecov Report

Merging #990 (567a39c) into main (138b159) will increase coverage by 0.75%.
The diff coverage is 38.28%.

@@           Coverage Diff            @@
##            main    #990      +/-   ##
========================================
+ Coverage   9.13%   9.89%   +0.75%     
========================================
  Files       1036    1042       +6     
  Lines      70787   74549    +3762     
  Branches   14781   17219    +2438     
========================================
+ Hits        6469    7375     +906     
- Misses     59008   60870    +1862     
- Partials    5310    6304     +994     

Flag	Coverage Δ		*Carryforward flag
differential	10.11% <36.52%> (?)
total	9.12% <57.14%> (ø)		Carriedforward from 138b159

*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% <ø> (ø)
...vior_velocity_planner/include/utilization/util.hpp	5.30% <ø> (+1.55%)	⬆️
...autoware_utils/test/src/geometry/test_geometry.cpp	44.27% <27.45%> (+4.83%)	⬆️
...ils/test/src/trajectory/test_path_with_lane_id.cpp	27.65% <27.65%> (ø)
...ware_utils/test/src/trajectory/test_trajectory.cpp	38.08% <30.29%> (-3.59%)	⬇️
...ware_utils/geometry/path_with_lane_id_geometry.hpp	33.33% <33.33%> (ø)
...t/src/geometry/test_path_with_lane_id_geometry.cpp	48.48% <48.48%> (ø)
...include/tier4_autoware_utils/geometry/geometry.hpp	90.60% <77.77%> (-1.45%)	⬇️
...r4_autoware_utils/trajectory/path_with_lane_id.hpp	83.33% <83.33%> (ø)
...ude/tier4_autoware_utils/trajectory/trajectory.hpp	90.77% <83.75%> (-2.33%)	⬇️
... 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 138b159...567a39c. Read the comment docs.

[takayuki5168]

velocity, acceleration, ... should be interpolated? What do you think.

[purewater0901]

Perhaps it's better to interpolate velocity, acceleration, and others outside of this function

[satoshi-ota]

Perhaps it's better to interpolate velocity, acceleration, and others outside of this function

I think so too.