Performance Improvement of graph_search

[ymd-stella]

In the case with few obstacles and a large number of edges, I observed that the computation time increases.

In ProbRoadmapGraph::createGraph, random points are sampled and all edges that do not collide are searched for.

teb_local_planner/teb_local_planner/src/graph_search.cpp

Line 343 in e4562a6

DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);

The following changes address this issue. Perform the probabilistic search so that it takes a finite number of searches to finish.

ymd-stella@b31bc11

Is there a better way to address this issue?