def getFullPath(self, mapref, start, goal, poss_goals, heatmap):
#returns cost and path
all_goal_coords = [goal] + poss_goals
goal_obs = d.generateGoalObs(mapref, start, all_goal_coords)
rmp, argmin = d.rmp(mapref, start, goal_obs)
target = d.findTarget(mapref, rmp, argmin, goal, heatmap)
#return self.customAstar(mapref, start, target, argmin.coord, goal, True) #returns to rmp only
cost1, path1 = self.customAstar(mapref, start, target, argmin.coord, goal, True)
cost2, path2 = mapref.optPath(target, goal, 2)
return cost1 + cost2, path1[1:] + path2[1:]
def step_gen(self):
#print "running generator"
all_goal_coords = [self.real_goal] + self.poss_goals
goal_obs = d.generateGoalObs(self.mapref, self.start, all_goal_coords)
rmp, argmin = d.rmp(self.mapref, self.start, goal_obs)
heatmap = d.HeatMap(self.mapref, goal_obs)
target = d.findTarget(self.mapref, rmp, argmin, self.real_goal, heatmap)
path1 = self.mapref.optPath(self.start,target)
path2 = self.mapref.optPath(target, self.real_goal)
path = path1[1:] + path2[1:]
self.path2 = path2
for step in path:
yield step