mission_planner fails when enable_correct_goal_pose param is true

[beyzanurkaya]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

There are 2 reasons for this problem:

1. The route_handler does not check for overlap lanelets for goal_lanelet, it only checks the closest lanelet to the goal_pose.
2. When enable_correct_goal_pose is true, the goal_pose is calculated with the closest lanelet to it, disregarding the overlap lanelets. Since this lanelet is not found in the all_route_lanelets obtained from the route_handler, the error message Goal is not valid! Please check position and angle of goal_pose is sent. No route is being generated.

overlap-lanes.mp4

Expected behavior

All overlap lanelets should be checked to generate a proper route, the shortest among the possible routes should be selected, and the route should be generated.

Actual behavior

There is a proper route available based on the given start and goal positions, but it is not being generated.

Steps to reproduce

1. set enable_correct_goal_pose to true
2. download map.zip
3. run Psim
4. give a goal pose on red dot with a single click
   

Versions

No response

Possible causes

No response

Additional context

No response

[kosuke55]

@beyzanurkaya

All overlap lanelets should be checked to generate a proper route, the shortest among the possible routes should be selected, and the route should be generated.

thanks, I agree with your idea! Do you plan to work on this task?

[kosuke55]

@beyzanurkaya
wow, thanks the PR #5533!!

[stale]

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