feat(auto_parking): automated valet parking implementation

[4swinv]

Description

This PR contains the initial implementation for the avp module.
The auto_parking module is made as a separate node under planning. The avp consists of two phases, 1) setting a goal to the nearest parking lot, driving to parking lot and 2) searching for a free parking space once inside the parking lot. The module publishes appropriate goal poses during the different phases. The following nodes are required to be running for auto_parking_node:

• freespace_planner
• goal_planner and start_planner
• scenario_selector

and related modules. The scenario_selector directs either freespace_planner or goal_planner and start_planner to generate trajectory based on goal pose published by auto_parking.

Phase 1:
Depending on initial pose when the auto_parking feature is engaged, a goal pose is set at the exit of the parking lot. This pose is first found by getting nearest parking lot, parking spaces inside and lanelets inside the parking lot from the lanelet map. By ordering the lanelets based on routing distance from current position, the exit lanelet is found. Ideally the auto_parking feature should be engaged from outside parking lot so the ordering is proper. The ego vehicle then drives to parking lot exit lanelet, passing through the parking lot. This driving can be in LANEDRIVING or PARKING scenario depending on whether start pose is already inside a parking lot.

Phase 2:
Once inside the parking lot, while driving to exit lanelet goal, the algorithm searches for a free parking space using the astar. Once a free space is found a new goal is set and trajectory is generated by the freespace_planner as scenario is PARKING.

Auto Park Node Configurations

th_arrived_distance_m: 1.0 // threshold for arrival at goal
th_parking_space_distance_m: 10.0 // search parking spaces within this radius from ego
update_rate: 5.0 // AutoParking node timer rate per second
vehicle_shape_margin_m: 0.2 // Vehicle shape margin for astar search

# fpa configs
.
.
.

The auto_parking node is also integrated with rviz allowing you to trigger auto parking from the simulator interface. ENABLE activates the auto parking, clicking the STOP button disables it.

Demo:

• Multiple obstacles in parking space

target.webm

• Costmap only sees one obstacle in the beginning

target2.webm

• Dynamic avoidance when introducing sudden obstacles and re-planning

target3.webm

Related links

Related launch PR: https://github.com/autowarefoundation/autoware_launch/pull/949/files

This PR requires the updated astar implementation, related PR's include:

• perf(freespace_planning_algorithms): update the astar search algorithm #6526
• perf(freespace_planning_algorithms): update the astar search algorithm autoware_launch#911
• fix(freespace_planning_algorithms): fix issue of astar search planner not reaching goal pose #6562
• feat(freespace_planning_algorithms): add tests for updated astar algorithm #6623

The implementation working is also described here:

• https://tier4.atlassian.net/wiki/spaces/~117245171/pages/3072525734/Automated+Parking+Implementation+using+improved+free-space+planner

Tests performed

Tested on multiple start pose cases inside and outside parking lot. Multiple patterns of obstacles also tested. The rviz panel also accounts for when start pose is not initialized and shows right state after finishing autoparking.

Notes for reviewers

All PR's in related links are pre-requisites for this PR +
launch PR: https://github.com/autowarefoundation/autoware_launch/pull/949/files

Interface changes

New services, topics added for auto_parking. tier4_state_rviz_plugin modified.

Effects on system behavior

Allows automated parking from a valid start pose.

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.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[xmfcx]

@takayuki5168 @mkquda hello,

@NorahXiong from AutoCore will take over this task, do you have anything to share on this PR?

[mkquda]

@takayuki5168 @mkquda hello,

@NorahXiong from AutoCore will take over this task, do you have anything to share on this PR?

@xmfcx
Its great to have someone work on this feature, I'm curious is the intention to continue on the work done in this PR, or use it as reference point?

I think ideally it would be better to use this PR as reference for coming up with a well defined set of requirements and specifications for the AVP feature.

You can also refer to these previous discussions about AVP which can give some insight:

• https://github.com/orgs/autowarefoundation/discussions/5220
• https://github.com/orgs/autowarefoundation/discussions/5273

[xmfcx]

I was wondering if this could be revived with a small effort and got merged. But I see that this is a large scale project to enable AVP and we don't have an urgent need for this feature.

Along with the other parking related PRs, I thought this was a simple addition.

I think we can close this PR and just tag it as a reference under the discussions above.

cc. @mitsudome-r @NorahXiong @mkquda