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

• Imperfect detect collision strategy used in freespace planner #2353
• previous PR: fix: freespace_planner issue#2353 #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.

[xmfcx]

@NorahXiong could you look into this again?

[NorahXiong]

@NorahXiong could you look into this again?

@xmfcx Neither in the main branch nor the 0.40.0 version can the planning simulator work properly. I won't be able to recheck the bug until it is fixed.

It seems that I misunderstood the usage of the new planning simulator. I will try again.

[NorahXiong]

@xmfcx I reproduced the case using the latest code. The bug still exists.

It may be necessary to design some strategies to address such issues: for example, after generating a path, check whether the destination will be beyond the sensor detection range when the vehicle travels on the route. If it exceeds the detection range, abandon the current path and regenerate a new one; or when the destination is beyond the detection range, select an intermediate destination on the straight line between the ego vehicle and the destination. After the self-car reaches the intermediate destination, handle the remaining path, at such point, the destination should be within the detection range.

@NorahXiong could you look into this again?

@xmfcx Neither in the main branch nor the 0.40.0 version can the planning simulator work properly. I won't be able to recheck the bug until it is fixed.

It seems that I misunderstood the usage of the new planning simulator. I will try again.

[xmfcx]

@NorahXiong thanks for the assessment. Could you check if the change proposed in this PR solves this problem?

[NorahXiong]

@NorahXiong thanks for the assessment. Could you check if the change proposed in this PR solves this problem?

Not yet. The parking maneuver is still stuck in the corner case I posted before.

[xmfcx]

@NorahXiong does this PR at least improve anything? (Also could you resolve conflicts?)

[NorahXiong]

@NorahXiong does this PR at least improve anything? (Also could you resolve conflicts?)

@xmfcx I think it makes sense according to the PR description. My previous tests were conducted in an environment that was stashed before. Maybe the difference in test results was introduced by not completely updating the code as the corner case could not be reproduced in TakumIto's environment. I will update the whole project and test it with the latest code tomorrow after I finished the porting work.