This would greatly improve scalability with the number of robots. The challenging part is the focal heuristic, which needs to estimate the number (or "severity") of conflicts.
Changes needed:
Optional:
- in the
LowLevelPlan we should just return the sequence of motion primitives, not states and actions (this will also save memory)
- This will change the logic of checking for conflicts and constraints to be on the level of primitives, rather than states
The number of conflicts can then be estimated by using a linear search comparing two relevant primitives and increasing a counter for each collision.
Without the optional changes above, one would simple pick the t=g+m.t/2 as time (i.e., the middle of the primitive) and then check collisions similar to above one by one or by using a collision manager and increasing the num_contacts setting in FCL.
This would greatly improve scalability with the number of robots. The challenging part is the focal heuristic, which needs to estimate the number (or "severity") of conflicts.
Changes needed:
Optional:
LowLevelPlanwe should just return the sequence of motion primitives, not states and actions (this will also save memory)The number of conflicts can then be estimated by using a linear search comparing two relevant primitives and increasing a counter for each collision.
Without the optional changes above, one would simple pick the t=g+m.t/2 as time (i.e., the middle of the primitive) and then check collisions similar to above one by one or by using a collision manager and increasing the num_contacts setting in FCL.