Frenetix-Motion-Planner avoids goal region

[alessiogambi]

I have set up a scenario with multiple AVs, some of which have the "same" goal area (each one has its own planning problem, but their goal regions overlap). Execution is parallel. Planner is CPP.

The issue is that planners who arrive late at that "same" goal area avoid it instead of going for it. See the image below.

Do you have any idea why this is happening and how to fix it? My suspect is that the AVs (that completed already the scenario) might not have been removed from the scenario of the other ones (since there's no communication between the agents in different batches.

[alessiogambi]

A quick follow up. I tried the same scenario in different configurations:

• If only vehicle 10001 is there, it works
• If vehicle 10001 and another vehicle (10003) not traveling to the same goal region, it work
• If vehicle 10001 and another vehicle (10002) traveling to the same goal region, 10001 avoids the goal area.

I ran the CPP planner in parallel mode and checked the visibility of the obstacles (which is fine, obstacles disappear and are reported as so) and the trajectories rejected because of collisions (which are 0 because there are no obstacles). Therefore, my suspects now fall on the CostFunction

[alessiogambi]

I might have found something, so for the moment, I'll go with that.

Changing the value for prediction from 0.2 (original value) to 0.0 inside the frenetix-motion-planner/cost.yml configuration file makes the planner drive as expected.

[alessiogambi]

I also observed that the length of the road makes a difference (which if kinda of expected, but not intuitive). If we extend the lanelet after the goal area by 50, 100, or 150 meters, the agent reaches the goal area at different (increasing) speeds.