How to speed up DStarLite algorithm implementation (improvement)

[jrmses]

I have been using DStaLite and, in my profiles, most of the time was spent in "is_obstacle" function (13 secs out of 16 in a simple example) I have found a easy way to improve the time it spends in this critical function. In my laptop (3080) it passed from 16 sec to 3 secs.

You just need to:

change definition of self.obstacles_xy to a dictionary:

self.obstacles_xy = dict(((obstacle.x, obstacle.y),True) for obstacle in self.obstacles)

in "is_obstacle" now do:

def is_obstacle(self, node: Node):

is_in_obstacles= (node.x,node.y) in self.obstacles_xy

is_in_detected_obstacles = False if self.detected_obstacles_xy.shape[0] > 0: is_x_equal = self.detected_obstacles_xy[:, 0] == node.x is_y_equal = self.detected_obstacles_xy[:, 1] == node.y is_in_detected_obstacles = (is_x_equal & is_y_equal).any()

return is_in_obstacles or is_in_detected_obstacles