Imperfect detect collision strategy used in freespace planner

[NorahXiong]

Checklist

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

Description

1. Planning succeeded.
2. First segment of trajectory traveled normally.
3. Part of second segment of trajectory is out of costmap when arriving at the end of first segment so the system will replan but it finally stuck because the goal point is out of costmap too.

https://user-images.githubusercontent.com/103234047/203288355-ca4593a1-2739-43df-b559-42ebdb42d039.mp4

Expected behavior

The vehicle arrives at the goal or the path is successfully re-planned.

Actual behavior

The parking process stuck.

Steps to reproduce

1. Start autoware planning simulator using default. (select astar algorithms)

2. Set start and goal position using the commands below.

   • ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0.0, nanosec: 0.0}, frame_id: map}, pose: {pose: {position: {x: 3713.99, y: 73732.0, z: 0.664812}, orientation: {x: 0.0, y: 0.0, z: 0.850511, w: 0.525957}}, 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: 3746.88, y: 73748.5, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.847164, w: 0.531332}}}"

3. Engage the vehicle.

Versions

• OS: Ubuntu 20.04
• ROS2: Galactic
• Autoware: Autoware.universe:main

Possible causes

Demonstrated in issue description part.

Additional context

Expand the costmap size could temporarily solve the problem.

https://user-images.githubusercontent.com/103234047/203290907-6aa3dad7-7a51-47e6-addb-127b4027ba1b.mp4

No response

[satoshi-ota]

@TakaHoribe Could you check this issue?

[stale[bot]]

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

[yaoweixiao-ux]

I want know how can I using topic control "engage"

[TakumIto]

@NorahXiong Could you share the map used in the simulation?

[NorahXiong]

@NorahXiong Could you share the map used in the simulation? It is the Autoware sample map. I think you can find it in the tutorial website. Anyway, the vector map is pasted below for you. lanelet2_map.zip

[stale[bot]]

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