One possible heuristic is based on the lanelet id: Retrieving the lanelet of the current state and checking whether it matches the lanelet id of the goal. We can reward states with the same id and therefore improve the heuristic.
I tried to implement this but find it hard to validate my approach. First, I wanted to check if the current state’s lanelet id matches any of the goals’. If not, I increase the heuristic cost by some factor
that I would need to adjust by trial and error, therefore making states with non-matching lanelet IDs more “costly”.
someFactor = 1.0
# list_ids_lanelets_goal is a list containing the lanelet IDs of the goal
if not self.list_ids_lanelets_goal is None:
# find lanelet id of current state using find_lanelet_by_position()
position = node_current.list_paths[-1][-1].position
x = position[0]
y = position[1]
point = np.array([x, y])
li = [point]
currentLanelet = self.scenario.lanelet_network.find_lanelet_by_position(li)
if not any(id in currentLanelet for id in self.list_ids_lanelets_goal):
someFactor = 1.2
...
# calculating some other costs
...
return cost * factor
However, I do not observe any performance improvements and time outs occur just as frequently without this. Am I doing some obvious thing wrong here?