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

libMultiRobotPlanning/example/mapf_prioritized_sipp.cpp

Line 241 in 4c75fa2

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

to

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

It should work. The result is shown below:

pp_sipp.mp4

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]

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