def bestroute(pos, dest):
rte = []
startwp, startdist = vclosestwp(pos)
endwp, enddist = vclosestwp(dest)
# print("wpts",startwp, endwp)
# if near to a known path, use one of it's path's wpts although
# closest actual waypt might be shorter. paths are safer
wp1, wp2 = astar2.nearpath(pos)
if (wp1 != 0): # path(s) close by, two waypt candidates each
dist1, route1 = astar2.astar(wp1, endwp)
print("bestroute1", route1)
dist2, route2 = astar2.astar(wp2, endwp)
print("bestroute2", route2)
if (dist1 < dist2): # which of the two wpts is closer
startwp = wp1
rte = route1
else: # not near any path
startwp = wp2
rte = route2
startdist = vmag(vsub(pos, waypts[startwp]))
else: # overland path
dist, rte = astar2.astar(startwp, endwp)
if startdist < 3.0: # if very close to starting point
rte.pop(0)
return rte
sendit("{c---}")
odometer(speed)
speed = 0
robot.motor(speed, steer)
else:
startwp, startdist = vclosestwp(posAV)
cstr = "startwp, dist %d, %5.2f " % (startwp,
startdist)
logit(cstr)
if wpt == 1: # <<<<<<< RTB >>>>>>>>>
route = bestroute(posAV, waypts[75])
elif (wpt > 1 and wpt < 5): # start of route
rtewp = routes[wpt][0] # 0th wpt in route
dist, route = astar2.astar(
startwp, rtewp) # goto start of route
route2 = routes[wpt]
route2.pop(0) # delete common wpt
route += route2
elif (wpt >= 10 and wpt <= 76): #start of waypoint
route = bestroute(posAV, waypts[wpt])
if len(route) > 0:
if startdist < 3.0: # if too close to starting waypoint
route.pop(0)
logit("start is close, advancing route")
if len(
route
) > 1: # if start between wpts & within 3 ft.
rwp0 = route[0]
def creervoiture(self):
self.car = self.firstview.car
if not self.car.position:
astar2.astar(self.chemin, self.car)
self.affichagefentresimu()
# order, distance = tsp.greedyTravelingWithSwaps(nodes, 0)
# tsp.plotTSP([order], points)
sa = SimAnnealing.SimAnneal(points, stopping_iter=5000)
sa.anneal()
sa.visualize_routes()
order = sa.best_solution
megaPath = []
totalCost = 0
print(target_xs)
print(target_ys)
for i in range(len(target_xs) - 1):
print("Returning", i)
start = (target_ys[order[i]], target_xs[order[i]])
end = (target_ys[order[i + 1]], target_xs[order[i + 1]])
#print("\n!_!_!_!_!_!")
path, cost = astar.astar(travel_friction, start, end)
megaPath += path
totalCost += cost
print(megaPath)
red = makeRed(travel_friction)
# display_img(red)
drawn = colorPath(red, megaPath)
display_img(drawn)
print(totalCost)
megaPath = [megaPath[i][0] for i in range(len(megaPath))]
#print(megaPath)
print("Score:", evaluate_path(travel_friction, target_points, megaPath))