Assertion triggering on simple crossing scenario

[ct2034]

On this scenario

{agents: [{goal: [0, 1], name: agent0, start: [3, 2]}, {goal: [0, 1], name: agent1,
      start: [3, 2]}, {goal: [1, 3], name: agent2, start: [2, 0]}], map: {dimensions: [
      4, 4], obstacles: []}}

calling with ecbs -i in.yaml -o out.yaml -w 1.5 I get this output:

ecbs: /... /libMultiRobotPlanning/include/libMultiRobotPlanning/ecbs.hpp:258: bool libMultiRobotPlanning::ECBS<State, Action, Cost, Conflict, Constraints, Environment>::search(const std::vector<T>&, std::vector<libMultiRobotPlanning::PlanResult<State, Action, Cost> >&) [with State = State; Action = Action; Cost = int; Conflict = Conflict; Constraints = Constraints; Environment = Environment]: Assertion `!newNode.constraints[i].overlap(c.second)' failed.

Tested on d3cfb64c27b448460244ee5ccf3e53d4f324ea5c

[whoenig]

Thanks for the detailed report. I can reproduce and I added a test to CI that triggers the issue, too (see https://github.com/whoenig/libMultiRobotPlanning/runs/4413006995?check_suite_focus=true).

This also happens with CBS and -w 1.0.

Note that the input is not valid (start and goal of agent0 and agent1 are identical), but this should be handled properly, of course.

[whoenig]

Fixed in 6fea197d715050269dbef765926e0734f5264b2c (that is, this change includes a proper check for valid input now).