Fix visibility graph shortest path algorithm

[AhmedAredah]

Describe the bug
find shortest path gives error and does not find the shortest path given stand and end point involving obstacles in between.

To Reproduce
Steps to reproduce the behavior:

1. use stand and end points not visible to each other

Expected behavior
The expected behavior should give a route of multiple waypoints/links connecting the start and end points.

Desktop (please complete the following information):

• OS: any OS
• Version v0.0.2