Improve the code quality and poor document of autoware_trajectory package

[soblin]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

As described in the Purpose section, this package needs to be improved to align with core quality.

Also usage of raw Path message types like PathWithLaneId have caused a lot of bugs, so this class will cover all Path related feature so that it can replace motion_utils package.

Purpose

Current documentation of autoware_trajectory does not contain rich information like this page.

https://autowarefoundation.github.io/autoware_core/main/common/autoware_lanelet2_utils/

The series of PRs aims to enrich the documentation so that the developers can understand the geometric picture of complicated Path related algorithms, and similar functions are not reproduced across Autoware.

Possible approaches

Here are part of the duplicate function/logic related to Path manipulation.

concatenate trajectory

• run_out
  • extendPath
• goal_planner
  • generatePullOverPath
  • PullOverPath::create
  • cropPath
  • cropForwardPath
• path_smoother
  • ElasticBandSmoother::extendTrajectory
• obstacle_cruise_planner
  • getExtendTrajectoryPoint
• bpp_common
  • combinePath

filter lane_ids

• bvp_common
  • getLaneletsIdSetOnPath
  • getLaneIdSetOnPath
  • getSortedLaneIdsFromPath
• blind_spot
  • struct InterpolatedPathInfo
• intersection
  • findLaneIdsInterval
• bpp_common
  • cutOverlappedLanes

assign new lane_ids

• goal_planner
  • ShiftPullOver::generatePullOverPath
• bpp_common
• convertWaypointsToPathWithLaneId

generate new Path from Lanelet centerline

• bpp_common
  • generateCenterLinePath
  • getCenterlinePath, getCenterlinePathFromLanelet
  • calcCenterLinePath L32
• route_handler
  • generateCenterLinePath
• goal_planner
  • ShiftPullOver::generateReferencePath

find the position which is specific meters away from a given position

• bvp_common
  • arc_lane_utils::findForwardOffsetSegment
  • findBackwardOffsetSegment
  • findOffsetSegment
  • util.hpp
    • getAheadPose
• path_sampler
  • getPointIndexAfter
• obstacle_velocity_limiter
  • calculateStartIndex
• path_optimizer
  • getPointIndexAfter
• obstacle_cruise_planner
• getIndexWithLongitudinalOffset
• bpp_common
  • calcLongitudinalOffsetStartPoint, calcLongitudinalOffstGoalPoint

check if ego is over a specific position

• intersection
  • isOverTagetIndex
• bvp_common
  • isOverLine
• virtual_traffic_light
  • VirtualTrafficLightModule::isBeforeStopLine
  • VirtualTrafficLightModule::isAfterAnyEndLine

concatenate footprint polygons along path

• bvp_common
  • generatePathPolyogn
• sampler_common
  • buildFootprintPoints
• out_of_lane
  • calculate_trajectory_footprints
• bpp_common
  • create_extended_polygon_along_path

trim a part of path

• bvp_common
  • generatePathPolyogn
• sampler_common
  • buildFootprintPoints
• out_of_lane
  • calculate_trajectory_footprints
• bpp_common
  • create_extended_polygon_along_path

Definition of done

This task continues until motion_utils package gets almot unnecessary.