diff --git a/PathPlanning/VoronoiRoadMap/dijkstra_search.py b/PathPlanning/VoronoiRoadMap/dijkstra_search.py index 503ccc342e..68df9e5b81 100644 --- a/PathPlanning/VoronoiRoadMap/dijkstra_search.py +++ b/PathPlanning/VoronoiRoadMap/dijkstra_search.py @@ -59,7 +59,7 @@ def search(self, sx, sy, gx, gy, node_x, node_y, edge_ids_list): break elif not open_set: print("Cannot find path") - break + return [], [] current_id = min(open_set, key=lambda o: open_set[o].cost) current_node = open_set[current_id] diff --git a/tests/test_voronoi_dijkstra_search.py b/tests/test_voronoi_dijkstra_search.py new file mode 100644 index 0000000000..ee043b675a --- /dev/null +++ b/tests/test_voronoi_dijkstra_search.py @@ -0,0 +1,18 @@ +import conftest # Add root path to sys.path +from PathPlanning.VoronoiRoadMap.dijkstra_search import DijkstraSearch + + +def test_unreachable_goal_returns_empty_path(): + node_x = [0.0, 1.0, 2.0] + node_y = [0.0, 0.0, 0.0] + edge_ids_list = [[1], [0], []] + + rx, ry = DijkstraSearch(False).search( + 0.0, 0.0, 2.0, 0.0, node_x, node_y, edge_ids_list) + + assert rx == [] + assert ry == [] + + +if __name__ == '__main__': + conftest.run_this_test(__file__)