Controller outputs way too high velocity/acceleration

[VRichardJP]

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

I am debugging a freespace planner algorithm with the planning simulator and encountered this crazy situation:

Screencast from 12-22-2023 01_42_39 PM.webm

Like astar and other algorithms, the output path of the algorithm is transformed into a trajectory by freespace_planner using a fixed speed (default 5km/h):

https://github.com/autowarefoundation/autoware.universe/blob/3a0fd31b95852dedc9a1b26c5496cf31a4e26347/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp#L166-L178

The simulation goes usually well, but for some reason this time, the controller started to panic around the goal point and kept outputing very large velocity/acceleration targets.

I am not sure what triggered that behavior. I just remember the vehicle was stuck nearby the goal pose for a while before (but that's my algorithm.)

Expected behavior

Velocity/Acceleration should be capped

Actual behavior

velocity/acceleration diverge

Steps to reproduce

No idea

Versions

No response

Possible causes

No response

Additional context

No response

[VRichardJP]

I can reproduce the behavior rather consistently in my environment even with A* by placing an obstacle in front of the vehicle next to a turn point, like so:

Screencast from 01-12-2024 10_13_43 AM.webm

However I don't get the same effect with current main branch.

[VRichardJP]

When I visualize trajectory follower and vehicle cmd gate target acceleration or speed I don't see any crazy acceleration:

Deceleration values are pretty big, but it is supposed to make the vehicle stop, not go crazily fast.

I suspect this behavior only exists in the planning simulator, as a normal car would not be able to accelerate that much or instantly switch gears DRIVE <-> REVERSE

[VRichardJP]

I think the issue is partly due to the replan logic here:

https://github.com/autowarefoundation/autoware.universe/blob/b51759fa17f40a230749217e0113eea2b5d67657/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp#L444-L457

When isPlanRequired() is true due to an obstacle on the path or if the vehicle is far from the current trajectory, a new trajectory is generated while the vehicle is still moving (supposedly trying to stop). I am not sure it is a good idea. In particular, by the time the new trajectory is computed, the vehicle may not even be on it. For example in the case the obstacle is in front of the current trajectory, the new trajectory will likely start with a backward manoeuver (away from the obstacle) while the vehicle kept moving forward.

I have added a simple check on the current motion state, so that a new trajectory is only generated if the vehicle is stopped. The crazy acceleration behavior disappeared after.

[stale[bot]]

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