fix(freespace_planner): fix a bug that goal goes out of the costmap while parking.

[TakumIto]

Description

Freespace-planner succeeds to find a path to the goal. But the vehicle stops tracking trajectory because the goal point goes out of the costmap.

I fixed this issue by modifying the planning algorithm.
when searching the path, The planner checks if the relative position of the goal is out of the costmap or not.

Related links

issue#2353
previous PR PR#4502

Tests performed

• video: issue2353_compress.webm

• Newly-made path (previous one is shown in issue#2353)
  

• With the costmap
  

Effects on system behavior

Freespace-planner makes a different path from before one.

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.

[kosuke55]

@NorahXiong If you could check the PR here it would be very helpful.

[NorahXiong]

@NorahXiong If you could check the PR here it would be very helpful.

I think the PR did not totally resolve the problem as no dynamic-size costmap strategy is implemented.
Have the goal ever run out of the costmap during the test? (Not sure about that point, maybe a test video could be pasted.)

[TakumIto]

@NorahXiong The previous algorithm searches a path only included in the initial costmap but actually as the car moves, so does the cost map. Then the goal position sometimes goes out of the costmap.
This PR solves it so that the goal is never missed even if the map moves with the car.
I think we don't have to implement a dynamic-size cost map.

[NorahXiong]

@NorahXiong The previous algorithm searches a path only included in the initial costmap but actually as the car moves, so does the cost map. Then the goal position sometimes goes out of the costmap. This PR solves it so that the goal is never missed even if the map moves with the car. I think we don't have to implement a dynamic-size cost map.

The modification of the collision detection method is a little confusing to me. There might be small logical flaws cause it is more like a specific-bug-oriented patch. Actually, this PR did resolve the case in the original issue but I constructed another corner case as below:

1. Put an obstacle to force the trajectory planned.
2. The parking motion stuck in the last trajectory segment while the goal going out of costmap.

The initial and goal poses are pasted here in case you want to reproduce.

• ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0.0, nanosec: 0.0}, frame_id: map}, pose: {pose: {position: {x: 3719.052734375, y: 73735.5078125, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.23948639471352365, w: 0.9708997202322793}}, covariance: [1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1.]}}"

• ros2 topic pub --once /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0.0, nanosec: 0.0}, frame_id: map}, pose: {position: {x: 3751.814453125, y: 73737.5390625, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: -0.9681636893299471, w: 0.25031793915543815}}}"

[TakumIto]

@NorahXiong Thank you for checking my fix.
In my environment(in the revised program), your new corner case doesn't occur and any varid path is not found.
Before this revising, we found an invalid path and the ego moved to a position where the goal was not unreachable.
After this PR, invalid paths are not found anyway. But in terms of preventing inappropriate paths before they happen, I think this is not degrading. How about your idea.

[TakumIto]

And In my opinion dynamical-size costmap is not appropriate. Because In practical situations, the perception area is limited by sensor limitations and obstacles. So in parking, the goal position should be in the recognizable area: fixed-size costmap according to egos performance.

[NorahXiong]

@NorahXiong Thank you for checking my fix. In my environment(in the revised program), your new corner case doesn't occur and any varid path is not found. Before this revising, we found an invalid path and the ego moved to a position where the goal was not unreachable. After this PR, invalid paths are not found anyway. But in terms of preventing inappropriate paths before they happen, I think this is not degrading. How about your idea.

@TakumIto You mean the test case I provided can not be reproduced? I produced the test case in the environment with your revision merged. Have you changed any parameters from defaults?

And In my opinion dynamical-size costmap is not appropriate. Because In practical situations, the perception area is limited by sensor limitations and obstacles. So in parking, the goal position should be in the recognizable area: fixed-size costmap according to egos performance.

I totally agree that neither a variable-size map nor a large-size map is a good solution to the problem and I still have no idea how to solve the problem with a reasonable strategy.

[stale]

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

[TakumIto]

@NorahXiong I'm sorry for not getting back to you sooner.
new_prob_compress.webm
This video is about trying to reproduce a new problem you mentioned, with this PR and the latest autoware. The path causing the error was not searched (no paths were found). I have not changed anything from the default settings. I think if autoware is updated, the problem will be solved like in these videos

[NorahXiong]

@NorahXiong I'm sorry for not getting back to you sooner. new_prob_compress.webm This video is about trying to reproduce a new problem you mentioned, with this PR and the latest autoware. The path causing the error was not searched (no paths were found). I have not changed anything from the default settings. I think if autoware is updated, the problem will be solved like in these videos

I tried again and the bug recurred. My code has been updated to Nov 2023. I think there are no commits related to trajectory searching algorithm afterwards. Maybe you could adjust the goal pose and try again. Parameters also have an influence on the searching result.

[TakumIto]

@NorahXiong
I checked the parameters and tried the bug in some situations, but it has not occurred.
I noticed my modification is for only the A* search. Are you using RRT algorithm for parking? I think that problem may occur if you are using RRT. Thus I should modify RRT algorithm.

[NorahXiong]

@NorahXiong I checked the parameters and tried the bug in some situations, but it has not occurred. I noticed my modification is for only the A* search. Are you using RRT algorithm for parking? I think that problem may occur if you are using RRT. Thus I should modify RRT algorithm.

I'm sure A* is used. Here's the configuration:

[TakumIto]

@NorahXiong Your configurations are completely the same as mine.

[NorahXiong]

@TakumIto The different test results may be related to the OS configuration. I can't reproduce the bug in this issue all the time. FYI, the following is my OS config.

Env Info:

• Ubuntu 22.04.3 LTS
• glibc 2.35

[stale]

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