Inter-agent collisions using Prioritized Planning using SIPP

[The-SS]

Hello,

I was trying out the package for multi-agent motion planning using SIPP but noticed the presence of collisions when trying to visualize the ouptut.

I tried test_mapf_prioritized_sipp.py with createVideo=True and got "COLLISION! (agent-agent) (...)" messages. One of the test cases that caused collisions is: test_mapfSimple1. Prioritized Planning using SIPP doesn't seem to account for collisions with the other obstacles.

[yucswx]

您好！       您发送的邮件已收到。谢谢。 祝您生活愉快！

[opan08]

I found the same bug. If you change https://github.com/whoenig/libMultiRobotPlanning/blob/4c75fa20c435c440d8b6bd6dc81668ddc7296ba0/example/mapf_prioritized_sipp.cpp#L241 to

            sipp_t::interval(lastState.second, solution.states[i].second));

It should work. The result is shown below:

https://github.com/user-attachments/assets/20a79d75-357f-4b50-8546-f94e090bdea8

The collision interval for each location should be [t_position, t_next_position], assuming robot stays on the position until next goal comes.

Another reason is that the sipp just considers the vertice conflict, not edge conflict.

[yucswx]

您好！       您发送的邮件已收到。谢谢。 祝您生活愉快！