Fix critical path deadlock issue

[UmangBR]

User story

1. As a SD
2. I need to slightly improve critical path detection
3. So that the robots not to deadlock when driving on a common path.

Acceptance criteria

• Critical path deadlock issue is fixed

Definition of done (DoD)

• Linter & Checker were performed and passed
• Code Review has been completed
• Code was merged in sprint release candidate
• Updates are written in issue comments
• Feature builds and tests successfully
• Acceptance by product owner
• User Story & Acceptance Criteria fulfilled

DoD general criteria

• Feature has been fully implemented
• Feature has been merged into the mainline
• All acceptance criteria were met
• Product owner approved features
• All tests are passing
• Developers agreed to release