def main(request): # get방식을 통해 체크항목을 읽어온다.
# example, CLR p.528
# makes a testing Graphes (should inquire from sqlite)
# DB 에서 G 를 읽고 , request.GET을 통해서 선택된 리스트를 읽는다
#print Dijkstra(G,'1')
# how we calculate when we want to go like 1,4,6,17
print shortestPath(G,'1','18')
return render_to_response('test.html')
def compute_shortest_path(self, from_, to_, length_penalty=0.0):
if self.image is None:
raise AttributeError("Load an image first!")
path = shortestPath(self.G, from_, to_, length_penalty=length_penalty)
return path
def find_path(graph, start, end):
try:
path = dijkstra.shortestPath(graph, start.key, end.key)
path = graph.convert(path)
return path
except Exception:
return None
def find_path(graph, start, end):
try:
path = dijkstra.shortestPath(graph, start.key, end.key)
path = graph.convert(path)
return path
except Exception:
return None
def make_path(self, obj):
try:
self.stop_move()
self.path = dijkstra.shortestPath(self.win.node_graph(), (self.x, self.y), (obj.x, obj.y))
self.start_move()
except KeyError:
self.stop_move()
def runDijkstra(nodeGraph, myID):
"""
Convert our graph implementation to one
usable by the dijkstra algorithym
"""
graphnew = {}
for key in nodeGraph.keys():
graphnew.update({key: {}})
for ele in range(len(nodeGraph[key])):
graphnew[key].update({nodeGraph[key][ele]: 1})
#print(graphnew)
#Initalize tempRouting Table
tempRoutingTable = {"destination": {}}
for key in nodeGraph.keys():
try:
tempPath = dijkstra.shortestPath(graphnew, str(myID), key)[1:]
except KeyError as e:
print(
"KeyError on Key: {}: Graph not complete, moving on".format(e))
break
tempEntry = {key: {"path": tempPath, "cost": len(tempPath)}}
tempRoutingTable['destination'].update(tempEntry)
#print(json.dumps(tempRoutingTable, indent=3))
with open(str(myID) + '.json', 'w') as f:
json.dump(tempRoutingTable, f, indent=3)
def compute_shortest_path(self, from_, to_, length_penalty=0.0):
if self.image is None:
raise AttributeError("Load an image first!")
path = shortestPath(self.G, from_, to_, length_penalty=length_penalty)
return path
def alt_path(self, src, dst):
distances = self.distances.copy()
for k, v in pairwise(self.paths[src][dst]):
distances[k][v] = distances[k][v] + 1
path = dijkstra.shortestPath(self.distances, src, dst)
self.add_path(src, dst, path)
return path
def calculateOptimizedGraph(self,Graph):
GraphAux= {}
FinalGraph={}
for i in range(1,21):
iString = str(i)
for j in range(1,21):
if i != j:
GraphAux = copy.copy(Graph)
jString = str(j)
pathIToJ = shortestPath(Graph,iString,jString)
self.deleteLinks(pathIToJ,GraphAux)
self.addLinksToFinalGraph(pathIToJ,FinalGraph)
pathIToJ2 = shortestPath(GraphAux,iString,jString)
self.addLinksToFinalGraph(pathIToJ2,FinalGraph)
self.setLinksAsCero(pathIToJ,Graph)
self.setLinksAsCero(pathIToJ2,Graph)
return FinalGraph
def find_shortest(self, origin, target, extra_vertices=None):
"""Find shortest path using given vertices and static level vertices."""
nodes = defaultdict(dict)
origin = getattr(origin, "loc", origin)
target = getattr(target, "loc", target)
# make a surrounding box (larger by 3 tiles in each direction than origin/target locations) as an
# optimization to make dijkstra algorithm faster
minx, maxx = min(origin.x, target.x), max(origin.x, target.x)
miny, maxy = min(origin.y, target.y), max(origin.y, target.y)
minx, maxx = max(0, minx - 3), min(xmax, maxx + 3)
miny, maxy = max(0, miny - 3), min(ymax, maxy + 3)
p1, p2 = Loc(minx, miny), Loc(maxx, maxy)
# for dijkstra path alg, consider origin and destination as passable
passable = lambda _loc: _loc in (origin, target) or not self.blocked(
_loc)
locs = [l for l in self if self.in_box((p1, p2), l)]
locs = filter(passable, locs)
for loc in locs:
nlst = filter(passable, self.neighbour_locs(loc))
for nloc in nlst:
dist = 1 if (nloc.x == loc.x or nloc.y == loc.y) else 1.5
nodes[loc][nloc] = dist
# log("nodes", pformat(dict(nodes)))
# find shortest path
# log("origin", origin)
# log("target", target)
try:
shortest = dijkstra.shortestPath(nodes, origin, target)
return shortest[1:]
except KeyError:
# path is blocked completely
return []
return
vertices = field.vertices + [origin, loc] + extra_vertices
# make nodes dictionary for dijkstra function; dict is of format
# {vertice1: {vertice2: distance}}
for vertice in vertices:
for vert2 in vertices:
if vertice != vert2:
path = self.path(vertice, vert2)
if not self.blocked(path):
distance = self.distance(vertice, vert2)
if vertice in nodes:
nodes[vertice][vert2] = distance
else:
nodes[vertice] = {vert2: distance}
if vert2 in nodes:
nodes[vert2][vertice] = distance
else:
nodes[vert2] = {vertice: distance}
def find_shortest(self, origin, target, extra_vertices=None):
"""Find shortest path using given vertices and static level vertices."""
nodes = defaultdict(dict)
origin = getattr(origin, "loc", origin)
target = getattr(target, "loc", target)
# make a surrounding box (larger by 3 tiles in each direction than origin/target locations) as an
# optimization to make dijkstra algorithm faster
minx, maxx = min(origin.x, target.x), max(origin.x, target.x)
miny, maxy = min(origin.y, target.y), max(origin.y, target.y)
minx, maxx = max(0, minx-3), min(xmax, maxx+3)
miny, maxy = max(0, miny-3), min(ymax, maxy+3)
p1, p2 = Loc(minx, miny), Loc(maxx, maxy)
# for dijkstra path alg, consider origin and destination as passable
passable = lambda _loc: _loc in (origin, target) or not self.blocked(_loc)
locs = [l for l in self if self.in_box((p1,p2), l)]
locs = filter(passable, locs)
for loc in locs:
nlst = filter(passable, self.neighbour_locs(loc))
for nloc in nlst:
dist = 1 if (nloc.x==loc.x or nloc.y==loc.y) else 1.5
nodes[loc][nloc] = dist
# log("nodes", pformat(dict(nodes)))
# find shortest path
# log("origin", origin)
# log("target", target)
try:
shortest = dijkstra.shortestPath(nodes, origin, target)
return shortest[1:]
except KeyError:
# path is blocked completely
return []
return
vertices = field.vertices + [origin, loc] + extra_vertices
# make nodes dictionary for dijkstra function; dict is of format
# {vertice1: {vertice2: distance}}
for vertice in vertices:
for vert2 in vertices:
if vertice != vert2:
path = self.path(vertice, vert2)
if not self.blocked(path):
distance = self.distance(vertice, vert2)
if vertice in nodes:
nodes[vertice][vert2] = distance
else:
nodes[vertice] = {vert2: distance}
if vert2 in nodes:
nodes[vert2][vertice] = distance
else:
nodes[vert2] = {vertice: distance}
def find_the_best_path(G,s,t):
path = shortestPath(G,s,t)
flag = True
score = 0
edges = []
for i in range(len(path))[1:]:
s,t = path[i-1],path[i]
if G[s][t] == 10e6: # cheking impossible result
flag = False
score += G[s][t]
edges.append((s,t))
return edges,score,flag
def button_pressed(event):
global start_point
if start_point is None:
start_point = (int(event.ydata), int(event.xdata))
else:
end_point = (int(event.ydata), int(event.xdata))
path = shortestPath(G,
start_point,
end_point,
length_penalty=length_penalty)
plt.plot(np.array(path)[:, 1], np.array(path)[:, 0], c=current_color)
start_point = end_point
def execute_query_7(db):
graph = {}
year = int(input("\nEnter year: "))
month = int(input("Enter month: "))
day = int(input("Enter travel date: "))
while validate_day_month_year(db,day,month,year):
year = int(input("\nReenter year: "))
month = int(input("Reenter month: "))
day = int(input("Reenter travel date: "))
vertexes = list(db.airports.find({}, { 'IATA_CODE': 1 } ))
for v in vertexes:
graph[v['IATA_CODE']] = {}
edges = db.flights.aggregate([
{ '$match': {'MONTH': month, 'DAY': day, 'YEAR': year, 'CANCELLED': 0, 'DIVERTED': 0} },
{ '$group': { '_id' : {'origin':"$ORIGIN_AIRPORT", 'destination':"$DESTINATION_AIRPORT", 'dist':"$DISTANCE"} } },
])
for i in edges:
graph[i['_id']['origin']][i['_id']['destination']] = i['_id']['dist']
orig = input("Enter origin airport: ").upper()
while validate_airport(db,orig):
orig = input("Invalid form for first Airport (IATA Code in form of 3 alphabetical characters), re-enter: ").upper()
dest = input("Enter destination airport: ").upper()
while validate_airport(db,dest):
dest = input("Invalid form for first Airport (IATA Code in form of 3 alphabetical characters), re-enter: ").upper()
try:
distances, path = shortestPath(graph,orig,dest)
print("\nFlight path from %s to %s" % (orig,dest))
path_str = path[0]
for v in range(1,len(path)):
path_str = path_str+" ----> "+path[v]
print(f"\n{path_str}")
print("\nTotal flight distance (miles): %d" % distances[dest])
head = path[0]
for index in range(1,len(path)):
data = list(db.flights.find({'ORIGIN_AIRPORT': head ,'DESTINATION_AIRPORT': path[index] ,'MONTH': month, 'DAY': day, 'YEAR': year, 'CANCELLED': 0, 'DIVERTED': 0}))
orig_name = db.airports.find_one({'IATA_CODE': head})['AIRPORT']
dest_name = db.airports.find_one({'IATA_CODE': path[index]})['AIRPORT']
print("\nFlight %s to %s ---- Distance: %d" % (orig_name, dest_name, graph[head][path[index]]))
print("\nFlight #\tDeparture Time\tArrival Time")
for ent in data:
print("%d\t\t|%d\t\t|%d" % (ent['FLIGHT_NUMBER'], ent['SCHEDULED_DEPARTURE'], ent['SCHEDULED_ARRIVAL']))
head = path[index]
except:
print("\nNo path between %s and %s" % (orig, dest))
def to_mode(self, mode):
'''
Get to the mode which you want to arive, use the dijkstra algorithm to find
the shortest path.
'''
cur_mode = self.get_cur_mode()
path = shortestPath(self.graph, cur_mode, mode)
if len(path) != 1:
cur = path.pop(0)
while len(path) != 0:
next = path.pop(0)
self.cmd(self.path_cmd[(cur, next)])
cur = next
cur_mode = cur
return cur_mode
def _gerar_indiretas(self):
'''
Cria e retorna um dicionário de conexões indiretas entre todos os pontos.
Preserva toda a variedade de rotas para um mesmo para de pontos.
'''
indiretas = {}
# Gera as conexões (rotas) indiretas para cada ponto existente em self._diretas
for i in self._diretas.keys():
for j in self._diretas.keys():
if i != j and j not in self._diretas[i]:
indiretas[(i,j)] = dijkstra.shortestPath(self._diretas, i, j)
# retorna o dicionário criado
return indiretas
def mouse_moved(event):
if start_point is None:
return
end_point = (int(event.ydata), int(event.xdata))
path = shortestPath(G,
start_point,
end_point,
length_penalty=length_penalty)
global current_path
if current_path is not None:
current_path.pop(0).remove()
current_path = plt.plot(np.array(path)[:, 1],
np.array(path)[:, 0],
c=current_color)
def pick_next_street(self):
from dijkstra import shortestPath
from pprint import pprint
import copy
street_graph = copy.deepcopy(self.federate.street_graph)
if self.federate.path_weight == "distance":
# don't need to do anything for this one
pass
elif self.federate.path_weight == "congestion":
street_graph = copy.deepcopy(self.federate.congestion_graph)
else:
pass
if self.federate.randomize_graph == 1:
for start in street_graph.keys():
dict = street_graph[start]
for end in dict.keys():
weight = dict[end]
weight += (random() * .0001)
dict[end] = weight
try:
path = shortestPath(street_graph,
self.street.end_node_idx,
self.dest)
except KeyError:
print "ERROR"
print "going from %d" % self.street.end_node_idx
print "to: %d" % self.dest
pprint(street_graph)
raise NoRoute
self.delete_self()
try:
path = self.node_path_to_streets(path)
return path[self.street.idx]
except IndexError:
print "ERROR"
print "going from %d" % self.street.end_node_idx
print "to: %d" % self.dest
pprint(street_graph)
pprint(path)
raise NoRoute
except KeyError:
return self.street
def start(self,
starting_tmc,
ending_tmc,
tmc_identification_file = "TMC_Identification.csv",
create_network_edges_file = True,
edge_output_file = "network_edges.csv"):
self.starting_tmc = starting_tmc
self.ending_tmc = ending_tmc
self.tmc_identification_file = tmc_identification_file
self.edge_input_file = edge_output_file
netGen = NetworkEdgesGenerator(tmc_identification_file, edge_output_file)
if create_network_edges_file :
netGen.createEdgesThenSaveToFile()
self.tmc_ref_data = netGen.getTMCRefData()
self.graph = Graph(self.edge_input_file)
self.shortest_path = shortestPath(self.graph.edges, self.starting_tmc, self.ending_tmc)
self.completePath()
def pick_next_street(self):
from dijkstra import shortestPath
from pprint import pprint
import copy
street_graph = copy.deepcopy(self.federate.street_graph)
if self.federate.path_weight == "distance":
# don't need to do anything for this one
pass
elif self.federate.path_weight == "congestion":
street_graph = copy.deepcopy(self.federate.congestion_graph)
else:
pass
if self.federate.randomize_graph == 1:
for start in street_graph.keys():
dict = street_graph[start]
for end in dict.keys():
weight = dict[end]
weight += (random() * .0001)
dict[end] = weight
try:
path = shortestPath(street_graph, self.street.end_node_idx,
self.dest)
except KeyError:
print "ERROR"
print "going from %d" % self.street.end_node_idx
print "to: %d" % self.dest
pprint(street_graph)
raise NoRoute
self.delete_self()
try:
path = self.node_path_to_streets(path)
return path[self.street.idx]
except IndexError:
print "ERROR"
print "going from %d" % self.street.end_node_idx
print "to: %d" % self.dest
pprint(street_graph)
pprint(path)
raise NoRoute
except KeyError:
return self.street
def main():
w, h = 9, 9
adjMatrix = [[None for x in range(w)] for y in range(h)]
adjMatrix = createMatrix(adjMatrix)
print(adjMatrix)
print(dijkstra.shortestPath(adjMatrix, 2, 7))
def spin(self):
# locate all markers on the object
if False:
max_angle = 60.0 / 180.0 * math.pi
min_z = math.cos(max_angle)
print "min_z: %s" % (min_z)
# big_box
# marker_list = [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31]
# small_box
# marker_list = [32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45]
# table
# marker_list = [3,4,5,6]
# c-beam
marker_list = [46, 47, 48, 49, 50, 51, 52, 53, 54, 55]
mtrans_matrix = []
mtrans_weights_ori = []
mtrans_weights_pos = []
for i in range(0, len(marker_list)):
mtrans_matrix.append([])
mtrans_weights_ori.append([])
mtrans_weights_pos.append([])
for j in range(0, len(marker_list)):
if i > j:
mtrans_matrix[i].append([])
mtrans_weights_ori[i].append([])
mtrans_weights_pos[i].append([])
else:
mtrans_matrix[i].append(None)
mtrans_weights_ori[i].append(None)
mtrans_weights_pos[i].append(None)
marker_lock = Lock()
self.visible_markers = []
def markerCallback(data):
marker_lock.acquire()
self.visible_markers = []
for m in data.markers:
if m.id in marker_list:
self.visible_markers.append(
(m.id, pm.fromMsg(m.pose.pose)))
marker_lock.release()
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, markerCallback)
while not rospy.is_shutdown():
marker_lock.acquire()
visible_markers_local = copy.copy(self.visible_markers)
marker_lock.release()
accepted_markers = []
accepted_markers_id = []
accepted_markers_weights_pos = []
accepted_markers_weights_ori = []
for m in visible_markers_local:
n = PyKDL.Frame(m[1].M) * PyKDL.Vector(0, 0, -1)
if n.z() > min_z:
accepted_markers.append(m)
accepted_markers_id.append(m[0])
# n.z() is in range (min_z, 1.0), min_z is the best for orientation and 1.0 i best for position
weight_pos = (n.z() - min_z) / (1.0 - min_z)
if weight_pos > 1.0 or weight_pos < 0.0:
print "error: weight==%s" % (weight_pos)
accepted_markers_weights_pos.append(weight_pos)
accepted_markers_weights_ori.append(1.0 - weight_pos)
print "visible: %s accepted markers: %s" % (
len(visible_markers_local), accepted_markers_id)
for i in range(0, len(accepted_markers)):
for j in range(i + 1, len(accepted_markers)):
if accepted_markers[i][0] > accepted_markers[j][0]:
idx1 = i
idx2 = j
else:
idx1 = j
idx2 = i
# print " %s with %s"%(accepted_markers[idx1][0], accepted_markers[idx2][0])
T_C_M1 = accepted_markers[idx1][1]
T_C_M2 = accepted_markers[idx2][1]
T_M1_M2 = T_C_M1.Inverse() * T_C_M2
marker_id1_index = marker_list.index(
accepted_markers[idx1][0])
marker_id2_index = marker_list.index(
accepted_markers[idx2][0])
mtrans_matrix[marker_id1_index][
marker_id2_index].append(T_M1_M2)
mtrans_weights_ori[marker_id1_index][
marker_id2_index].append(
accepted_markers_weights_ori[idx1] *
accepted_markers_weights_ori[idx2])
mtrans_weights_pos[marker_id1_index][
marker_id2_index].append(
accepted_markers_weights_pos[idx1] *
accepted_markers_weights_pos[idx2])
rospy.sleep(0.1)
G = {}
for i in range(0, len(marker_list)):
for j in range(0, len(marker_list)):
if mtrans_matrix[i][j] == None:
pass
# print "%s with %s: None"%(marker_list[i], marker_list[j])
elif len(mtrans_matrix[i][j]) < 10:
print "%s with %s: %s (ignoring)" % (
marker_list[i], marker_list[j],
len(mtrans_matrix[i][j]))
else:
score, R_M1_M2 = velmautils.meanOrientation(
mtrans_matrix[i][j], mtrans_weights_ori[i][j])
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(
R_M1_M2,
mtrans_matrix[i][j][measure_idx]).rot.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances,
key=operator.itemgetter(0))
mtrans = []
weights_ori = []
for measure_idx in range(
0, int(0.8 * len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][
distances_sorted[measure_idx][1]])
weights_ori.append(mtrans_weights_ori[i][j][
distances_sorted[measure_idx][1]])
# score, R_M1_M2 = velmautils.meanOrientation(mtrans, weights_ori)
print "%s with %s: %s, score: %s" % (
marker_list[i], marker_list[j],
len(mtrans_matrix[i][j]), score)
P_M1_M2 = velmautils.meanPosition(
mtrans_matrix[i][j], mtrans_weights_pos[i][j])
print "P_M1_M2 before: %s" % (P_M1_M2)
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(
R_M1_M2,
mtrans_matrix[i][j][measure_idx]).vel.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances,
key=operator.itemgetter(0))
mtrans = []
weights_pos = []
for measure_idx in range(
0, int(0.8 * len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][
distances_sorted[measure_idx][1]])
weights_pos.append(mtrans_weights_ori[i][j][
distances_sorted[measure_idx][1]])
# P_M1_M2 = velmautils.meanPosition(mtrans, weights_pos)
print "P_M1_M2 after: %s" % (P_M1_M2)
mtrans_matrix[i][j] = PyKDL.Frame(
copy.deepcopy(R_M1_M2.M), P_M1_M2)
neighbours = G.get(marker_list[i], {})
if score > 0.01:
score = 1000000.0
else:
score = 1.0
neighbours[marker_list[j]] = score
G[marker_list[i]] = neighbours
neighbours = G.get(marker_list[j], {})
neighbours[marker_list[i]] = score
G[marker_list[j]] = neighbours
frames = []
# get the shortest paths weighted by score from optimization
for marker_id in marker_list:
path = dijkstra.shortestPath(G, marker_id, marker_list[0])
print path
T_Ml_Mf = PyKDL.Frame()
for i in range(0, len(path) - 1):
if marker_list.index(path[i]) > marker_list.index(
path[i + 1]):
T_Mp_Mn = mtrans_matrix[marker_list.index(
path[i])][marker_list.index(path[i + 1])]
T_Ml_Mf = T_Ml_Mf * T_Mp_Mn
else:
T_Mn_Mp = mtrans_matrix[marker_list.index(
path[i + 1])][marker_list.index(path[i])]
T_Ml_Mf = T_Ml_Mf * T_Mn_Mp.Inverse()
frames.append(copy.deepcopy(T_Ml_Mf.Inverse()))
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
m_id = 0
marker_idx = 0
for fr in frames:
q = fr.M.GetQuaternion()
p = fr.p
print "[%s, PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s),PyKDL.Vector(%s,%s,%s))]," % (
marker_list[marker_idx], q[0], q[1], q[2], q[3], p.x(),
p.y(), p.z())
m_id = pub_marker.publishFrameMarker(fr,
m_id,
scale=0.1,
frame='world',
namespace='default')
rospy.sleep(0.1)
marker_idx += 1
rospy.sleep(2.0)
exit(0)
else:
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
markers2 = [
[
46,
PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0, 0.0, 0.0, 1.0),
PyKDL.Vector(-0.0, -0.0, -0.0))
],
[
47,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.0161417497092,
-9.92379339669e-05,
-0.00603105214296,
0.999851519216),
PyKDL.Vector(-0.0987990318312, -0.000265582834399,
0.000544628226204))
],
[
48,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.706829255712,
0.00114672638679,
-0.707103949357,
0.0198769487466),
PyKDL.Vector(0.0427261427894, 0.00245374838957,
-0.116698579624))
],
[
49,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.706230029879,
-0.00660144281521,
-0.70769079068,
0.0192174565616),
PyKDL.Vector(0.0410352237403, 0.000595788239244,
-0.0336956438972))
],
[
50,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.0155276433453,
0.714580229904,
0.00743395758828,
0.699341635824),
PyKDL.Vector(-0.131991504795, -0.00410930997885,
-0.0916799439467))
],
[
51,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.706160148032,
0.0114839861434,
-0.707838133957,
0.0130820300375),
PyKDL.Vector(-0.150701418215, -0.000688426198715,
-0.035762545196))
],
[
52,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.705850422242,
0.00260066465892,
-0.708005925475,
0.022271673869),
PyKDL.Vector(-0.150663515172, -0.00196494029406,
-0.123356224597))
],
[
53,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.00630495805624,
-0.999979097775,
0.000308879730173,
0.00139860956497),
PyKDL.Vector(-0.0116053782242, 0.000352840524022,
-0.0267278839783))
],
[
54,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.00853722085061,
-0.999853611966,
0.00230495916657,
0.0146477869582),
PyKDL.Vector(-0.0949889134574, 0.00131718330416,
-0.0165548984986))
],
[
55,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.0208301706621,
-0.711731154203,
0.0052945617051,
0.702123091589),
PyKDL.Vector(0.0244779541548, 0.000463479220587,
-0.0804749548707))
],
]
markers = [
[
46,
PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0, 0.0, 0.0, 1.0),
PyKDL.Vector(-0.0, -0.0, -0.0))
],
[
47,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.00211977224419,
0.00133104595822,
-0.00433238039783,
0.999987482603),
PyKDL.Vector(-0.0995553679408, -0.000651966932258,
0.000444468330964))
],
[
48,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.705539374795,
0.00129956423061,
-0.708394191186,
0.0197527628665),
PyKDL.Vector(0.0433256677966, 0.00212664651843,
-0.116482343501))
],
[
49,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.704707757517,
-0.00628228374428,
-0.709255128279,
0.0174548680028),
PyKDL.Vector(0.0412709849952, 0.000494665961165,
-0.0338667341872))
],
[
50,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.0117276773848,
0.706312439798,
0.00558379104701,
0.707781053891),
PyKDL.Vector(-0.131246837996, -0.00469319026484,
-0.0943814089463))
],
[
51,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.710112841604,
0.00963407546503,
-0.703895468304,
0.013345653983),
PyKDL.Vector(-0.149984963191, -0.00300459973807,
-0.0370193446712))
],
[
52,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.704691425649,
0.00497982940017,
-0.709159595229,
0.0218601100464),
PyKDL.Vector(-0.15277553848, -0.00431480401095,
-0.120995842686))
],
[
53,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.00984066000086,
-0.999945658681,
0.00299307949816,
0.00169781765667),
PyKDL.Vector(-0.0132269965227, -0.00110379001368,
-0.0274175040768))
],
[
54,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.00154140261629,
0.999633307109,
-0.00751679824708,
0.0259686953912),
PyKDL.Vector(-0.0974091549673, -0.000670004842722,
-0.0319416169884))
],
[
55,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.0195903374145,
-0.713404165076,
0.00273342374532,
0.700473585745),
PyKDL.Vector(0.0250633176495, -0.00356911439713,
-0.0811403242495))
],
]
m_id = 0
for m in markers:
print m[0]
print m[1]
m_id = pub_marker.publishFrameMarker(m[1],
m_id,
scale=0.1,
frame='world',
namespace='default')
rospy.sleep(0.1)
def shortest_path(board, start, end, test=is_nonwall):
"Return the shortest path between two points on the board."
return dijkstra.shortestPath(DijkstraGraph(board, test), start, end)
def assert_path_equals(self, start, end, expected_path, expected_distance):
path, distance = dijkstra.shortestPath(self.graph, start, end)
self.assertEqual('-'.join(path), expected_path, 'path')
self.assertEqual(distance, expected_distance, 'distance')
def processRouteRequest(src, destinations, junctionsDict, graphDict):
"""
- Transforms the source and destination coordinates to SUMO junctions ID
- Resolves the routing demand by using a dijkstra algorithm
- Sends the route (list of geographic coordinates) back to the client
"""
route = []
first = True
if isJunction(src):
#src become an edge predecessor of the src junction
src = iter(junctionsDict[getJunctionId(src)][0]).next()
srcJunction = True
else:
srcJunction = False
for dest in destinations:
if isJunction(dest):
#dest become an edge successor of the dest junction
dest = iter(junctionsDict[getJunctionId(dest)][1]).next()
destJunction = True
else:
destJunction = False
try:
#Getting shortest path
tmpRoute = dijkstra.shortestPath(graphDict, src, dest)
except Exception as e:
Logger.exception(e)
return constants.ROUTE_ERROR_CONNECTION, None
#Removing the first edge if it's the first routing and we find recurrence
if first and srcJunction and tmpRoute:
tmpRoute.pop(0)
elif first and len(tmpRoute) > 1 and tmpRoute[1] == getOppositeEdge(tmpRoute[0]):
tmpRoute.pop(0)
#Removing the last edge if it was a junctions from start
if destJunction and tmpRoute:
tmpRoute.pop()
#Removing the first edge of routings with via points in order to avoid two identical edges when extending road
if not first:
tmpRoute.pop(0)
#Adding the calculated routing to the main routing
route.extend(tmpRoute)
first = False
#Updating source edge for the next routing
if tmpRoute:
src = tmpRoute[-1]
else:
src = dest
if len(route) > 1 and route[-2] == getOppositeEdge(route[-1]):
route.pop()
return 0, route
#!/usr/bin/python
# -*- coding: UTF-8 -*-
import dijkstra
from lastgraph import LastGraph
if __name__ == '__main__':
g = LastGraph()
path = dijkstra.shortestPath(g,
g.key_for_band_name('david bowie'),
g.key_for_band_name('vanilla ice'))
for i in range(len(path) - 1):
print('%s -> %s (%f%% similarity)' %
(g[path[i]].get_name(),
g[path[i+1]].get_name(),
g[path[i]].get_similarity(path[i+1]) * 100))
def main(startLatitude, startLongitude, endLatitude, endLongitude, maxDistance, supportedChargers):
print("Main Start")
requestList = requestGenerator(startLatitude,startLongitude,endLatitude,endLongitude, supportedChargers)
print("generating graph")
graph = graphGenerator(requestList,maxDistance)
#shortestPath = dijkstra.shortestPath(graph,"start","end")
#actualDistances = googleNodes.googleMapsActualPath(requestList, shortestPath)
correctPath = False
#print(shortestPath)
count = 0
print("searching for best path")
while correctPath == False:
correctPath=True
currentDistanceCounter = 0
print("calculating path")
shortestPath = dijkstra.shortestPath(graph,"start","end")
#print("shortestPath {}".format(shortestPath))
if shortestPath == None or None in shortestPath or count>50:
if count>50:
print("too many trimming iterations")
print(shortestPath)
return None
break
else:
actualDistances=googleNodes.googleMapsActualPath(requestList,shortestPath)
print("actual distances of the way points in KM are {}".format(actualDistances))
print("verifying path")
while currentDistanceCounter<len(actualDistances):
if actualDistances[currentDistanceCounter]/1000>maxDistance:
correctPath = False
graph[str(shortestPath[currentDistanceCounter])].pop(str(shortestPath[currentDistanceCounter+1]),None)
else:
graph[str(shortestPath[currentDistanceCounter])][str(shortestPath[currentDistanceCounter+1])]=actualDistances[currentDistanceCounter]/1000
currentDistanceCounter += 1
# if(correctPath==True):
# break
#print(graph)
print("actual distances {}".format(actualDistances))
count+=1
if shortestPath==None:
print("No path found")
return None
print("Packaging results")
waypointsData = []
waypointsData.append([requestList[0][0],requestList[0][1],None,None])
for point in shortestPath:
if point != "start" and point != "end":
waypointsData.append([requestList[int(point)+1][0],requestList[int(point)+1][1],requestList[int(point)+1][3],requestList[int(point)+1][4]])
#print(requestList[int(point)+1])
waypointsData.append([requestList[1][0],requestList[1][1],None,None])
#print(waypoints)
waypoints = {}
waypoints['Waypoints']=waypointsData
return waypoints
#current Inputs
#startLatitude = 39.5
#startLongitude = -121.9
#endLatitude = 40
#endLongitude = -121
#maxDistance = 100
#supportedChargers = []
#waypoints = main(startLatitude,startLongitude,endLatitude,endLongitude,maxDistance,supportedChargers)
#print(waypoints)
#https://maps.googleapis.com/maps/api/distancematrix/json?units=imperial&origins=longtitude,latitude&destinations=40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.659569%2C-73.933783%7C40.729029%2C-73.851524%7C40.6860072%2C-73.6334271%7C40.598566%2C-73.7527626%7C40.659569%2C-73.933783%7C40.729029%2C-73.851524%7C40.6860072%2C-73.6334271%7C40.598566%2C-73.7527626&key=YOUR_API_KEY
def generate(self, subGoalRadius):
"""
Generates a series of random points that will become the
roadmap and connects them and weights them into a graph.
If the goal and the starting point are not connected, more
points are added. The roadmap is then searched for the shortest
weighted distance which become the intermediate goals.
@param subGoalRadius The radius of the intermediate goals
@return A list of sub goals from the roadmap connecting
the starting point and the end goal
"""
self.roadmap = dict()
currentPos = 0
self.dontDraw = list()
while len(self.gPosList) <= 1:
for i, j in enumerate(self.subGoalPositionList):
# adds the neighbours for a certain vertex to the its sub
# dictionary neighbours are decided by linear distance
if i >= currentPos:
self.roadmap[i] = dict()
for p, q in self.findNeighbors(j):
self.roadmap[i][p] = (
1000 * self.norm(j, q) /
min(
self.omegaDict[j],
self.omegaDict[q]
)
)
try:
self.roadmap[p][i] = self.roadmap[i][p]
except KeyError:
pass
self.screen.fill(
(255, 255, 255)
)
self.draw()
map(
lambda o: o.draw(),
self.obstacleList
)
pygame.display.flip()
for e in pygame.event.get():
if e.type is pygame.QUIT:
exit()
self.goalNodes = dijkstra.shortestPath(
self.roadmap,
0,
len(self.subGoalPositionList) - 1
)
self.gPosList = map(
lambda k: self.subGoalPositionList[k],
self.goalNodes
)
if len(self.gPosList) == 1:
currentPos = len(self.subGoalPositionList) - 1
self.dontDraw += [
currentPos,
currentPos - 1,
currentPos + 1,
]
newPosList = self.generatePositionList(
int(self.subGoalNumber / 2) + 1
)
self.initOmega(newPosList)
self.subGoalPositionList[1: -1] += newPosList
self.filterSubGoal()
#print self.roadmap
retList = map(
lambda p: goal.CircleGoal(
subGoalRadius,
p,
self.screen
),
self.gPosList
)
retList[-1].radius = 1 * subGoalRadius
return retList
def optimize(pid,w=1,h=1,eq=GeoUtils.constants.Equations.SMCDD,elevdata=GeoUtils.constants.ElevSrc.DEFAULT30SEC):
'''
Run Port Protector Optimization
Parameters
@param pid - port ID
@param w - grid width in degrees (default: 1)
@param h - grid height in degrees (default: 1)
@param eq - design equation as constant in GeoUtils.constants.Equations (default: SUPERSLR Minimum-Criteria Dike Design)
@param elevdata - elevation data to use for grid as constant in GeoUtils.constants.ElevSrc (default: 30-second SRTM30Plus)
'''
# Get grid
response,error = makeNetwork(int(pid),float(w),float(h),eq,elevdata)
# If there was an error, return error and message
if error == True:
errtxt = response
# Function exits
return errtxt,True
# Unpack response
(grid,graph,startpts,endpts,bPoly) = response
# Import shortest path algorithm
import dijkstra
# For each start point and end point
optimalPaths = [ dijkstra.shortestPath(graph,start,end) for end in endpts for start in startpts ]
# Initialize minimum distance to infinity
vol = float('inf')
# Initialize shortest path to false
sp = False
# For each path and distance, check to determine if shortest
for possiblePath in optimalPaths:
if possiblePath == (False,False):
pass
elif float(possiblePath[1]) < vol:
vol = float(possiblePath[1])
sp = possiblePath[0]
else:
pass
# Shortest path details variable holders
path = []
pts = []
elev = []
length = 0.0
# Initialize volume variables
dikeVol = 0.0
toeVol = 0.0
coreVol = 0.0
armorVol = 0.0
foundVol = 0.0
totalVol = 0.0
try:
prev = False
for v in sp:
# add up incremental volumes
if (eq == GeoUtils.constants.Equations.BMASW or eq == GeoUtils.constants.Equations.SMCDD) and prev:
dikeVol += graph[prev][v]['dikeVol']
toeVol += graph[prev][v]['toeVol']
coreVol += graph[prev][v]['coreVol']
foundVol += graph[prev][v]['foundVol']
armorVol += graph[prev][v]['armorVol']
totalVol += graph[prev][v]['cost']
# Add point details to paths
path.append(grid[v]["latlon"])
pts.append(grid[v]["metric"])
elev.append(float(grid[v]["elev"]))
# Set previous point to current point
prev = v
except TypeError:
# Build error message
msg = '<h3>Error:</h3>\n'
msg += '<p>No optimal paths found, please change your parameters and try again.</p>\n<br/><br/>\n'
msg += '<p>Debugging information (TypeError):<br/>%s</p>\n' % (sp,)
# Output error message
output = GeoUtils.Interface.uniForm.fullErrorMsgGen(msg)
# Return error and message
# Function exits
return output,True
# Average elevation along path
avg_elev = sum(elev) / len(elev)
# Prepare values used to update database
output = (path,avg_elev,totalVol,dikeVol,coreVol,toeVol,foundVol,armorVol)
# Return output and no error
return output,False
def getShortestPath(self, roadmap, fromNode, toNode):
return dijkstra.shortestPath(
roadmap,
fromNode,
toNode
)
def testLength(self):
self.assertTrue(
dj.shortestPath(self.length_paths, 57833).wkt ==
'LINESTRING (-77.0091764 38.9181108, -77.0091764 38.9169964, -77.0091764 38.9169964, -77.0091774 38.9168085, -77.0091774 38.9168085, -77.0100508 38.9165237, -77.01083 38.9162576, -77.01168629999999 38.9159488, -77.01168629999999 38.9159488, -77.01205040000001 38.9158167, -77.01215379999999 38.9157809, -77.01215379999999 38.9157809, -77.0122609 38.9157463, -77.01270220000001 38.9155971, -77.01270220000001 38.9155971, -77.01283909999999 38.9155948, -77.01307300000001 38.915591, -77.01324200000001 38.91559, -77.01343199999999 38.915569, -77.014259 38.915568, -77.01434 38.915569, -77.01434 38.915569, -77.014374 38.915886, -77.014374 38.915886, -77.014478 38.915887, -77.015399 38.915915, -77.015507 38.9159169, -77.015507 38.9159169, -77.0155099 38.9159543, -77.0155242 38.9159936, -77.0155362 38.9160128, -77.01556890000001 38.9160478, -77.0156022 38.9160708, -77.0156404 38.9160884, -77.01568140000001 38.9160998, -77.01571920000001 38.9161048, -77.0157578 38.9161049, -77.0157578 38.9161049, -77.0158003 38.9160991, -77.0158376 38.9160884, -77.0158587 38.9160797, -77.01587840000001 38.9160694, -77.0158966 38.9160575, -77.01591329999999 38.9160442, -77.0159408 38.9160143, -77.0159601 38.9159803, -77.0159671 38.9159592, -77.0159712 38.9159301, -77.0159712 38.9159301, -77.016075 38.915933, -77.016487 38.915941, -77.01680399999999 38.915949, -77.016836 38.915951, -77.016901 38.91595, -77.01693299999999 38.915947, -77.0170336 38.9159393, -77.01725399999999 38.915915, -77.017332 38.915909, -77.017332 38.915909, -77.017839 38.915859, -77.018553 38.915792)'
)
def shortestPath(self,start,end):
return dijkstra.shortestPath(self.edges, start, end)
def get_critical_activities(self, d_activities):
# warning = {}
# Read the activity details
activities = d_activities.values()
for start_activity in activities:
if start_activity.is_start:
break
l_successor_date_earliest_start = []
for successor in start_activity.successors:
if successor.date_earliest_start:
l_successor_date_earliest_start.append(
successor.date_earliest_start)
if l_successor_date_earliest_start:
start_activity.date_early_start = min(
l_successor_date_earliest_start)
else:
start_activity.date_early_start = network_activity.next_work_day(
datetime.today())
network_activity.walk_list_ahead(start_activity)
for stop_activity in activities:
if stop_activity.is_stop:
break
stop_activity.date_late_finish = stop_activity.date_early_finish
stop_activity.date_late_start = network_activity.sub_work_days(
stop_activity.date_late_finish, stop_activity.replan_duration)
network_activity.walk_list_aback(stop_activity)
start_activity.date_late_finish = start_activity.date_early_finish
start_activity.date_late_start = network_activity.sub_work_days(
start_activity.date_late_finish, start_activity.replan_duration)
# Calculate Float
for act in activities:
l_successor_date_early_start = []
for successor in act.successors:
l_successor_date_early_start.append(successor.date_early_start)
if l_successor_date_early_start:
[act.free_float, rr] = network_activity.work_days_diff(
act.date_early_finish, min(l_successor_date_early_start))
[act.total_float,
rr] = network_activity.work_days_diff(act.date_early_start,
act.date_late_start)
# Calculate shortest path
C_INFINITE = 9999
d_graph = {}
for act in d_activities.keys():
d_neighbours = {}
for other_act in d_activities.keys():
if other_act != act:
d_neighbours[other_act] = C_INFINITE
for pred_act in d_activities[act].predecessors:
if other_act == pred_act.activity_id:
d_neighbours[other_act] = pred_act.total_float
for succ_act in d_activities[act].successors:
if other_act == succ_act.activity_id:
d_neighbours[other_act] = succ_act.total_float
d_graph[act] = d_neighbours
l_spath = []
try:
l_spath = shortestPath(d_graph, 'start', 'stop')
except Exception:
_logger.warning(
"""Could not calculate the critical path due to existing negative floats
in one or more of the network activities.""")
for act in activities:
item = next((i for i in l_spath if i == act.activity_id), None)
if item is not None:
act.is_critical_path = True
def testSpeed(self):
self.assertTrue(
dj.shortestPath(self.speed_paths, 57833).wkt ==
'LINESTRING (-77.0091764 38.9181108, -77.0091764 38.9169964, -77.0091764 38.9169964, -77.0091774 38.9168085, -77.0091774 38.9168085, -77.0100508 38.9165237, -77.01083 38.9162576, -77.01168629999999 38.9159488, -77.01168629999999 38.9159488, -77.01205040000001 38.9158167, -77.01215379999999 38.9157809, -77.01215379999999 38.9157809, -77.0122609 38.9157463, -77.01270220000001 38.9155971, -77.01270220000001 38.9155971, -77.01427529999999 38.9150493, -77.01427529999999 38.9150493, -77.0149029 38.914826, -77.0157385 38.9145421, -77.0157385 38.9145421, -77.01667260000001 38.9142077, -77.0168939 38.9141285, -77.0168939 38.9141285, -77.0170688 38.9142041, -77.0171465 38.9142372, -77.0171465 38.9142372, -77.0180278 38.9146158, -77.0180278 38.9146158, -77.0182942 38.9147303, -77.0182942 38.9147303, -77.0184174 38.9147791, -77.0184174 38.9147791, -77.01841 38.914874, -77.018418 38.914922, -77.0184565 38.9151711, -77.01847100000001 38.915265, -77.01848 38.915331, -77.018553 38.915792)'
)
def getShortestPath(self, roadmap, fromNode, toNode):
return dijkstra.shortestPath(
roadmap,
fromNode,
toNode
)
def path(self, src, dst):
path = dijkstra.shortestPath(self.distances, src, dst)
self.add_path(src, dst, path)
return path
def generate(self, subGoalRadius):
"""
Generates a series of random points that will become the
roadmap and connects them and weights them into a graph.
If the goal and the starting point are not connected, more
points are added. The roadmap is then searched for the shortest
weighted distance which become the intermediate goals.
@param subGoalRadius The radius of the intermediate goals
@return A list of sub goals from the roadmap connecting
the starting point and the end goal
"""
self.roadmap = dict()
currentPos = 0
self.dontDraw = list()
while len(self.gPosList) <= 1:
for i, j in enumerate(self.subGoalPositionList):
# adds the neighbours for a certain vertex to the its sub
# dictionary neighbours are decided by linear distance
if i >= currentPos:
self.roadmap[i] = dict()
for p, q in self.findNeighbors(j):
self.roadmap[i][p] = (
1000 * self.norm(j, q) /
min(
self.omegaDict[j],
self.omegaDict[q]
)
)
try:
self.roadmap[p][i] = self.roadmap[i][p]
except KeyError:
pass
self.screen.fill(
(255, 255, 255)
)
self.draw()
map(
lambda o: o.draw(),
self.obstacleList
)
pygame.display.flip()
for e in pygame.event.get():
if e.type is pygame.QUIT:
exit()
self.goalNodes = dijkstra.shortestPath(
self.roadmap,
0,
len(self.subGoalPositionList) - 1
)
self.gPosList = map(
lambda k: self.subGoalPositionList[k],
self.goalNodes
)
if len(self.gPosList) == 1:
currentPos = len(self.subGoalPositionList) - 1
self.dontDraw += [
currentPos,
currentPos - 1,
currentPos + 1,
]
newPosList = self.generatePositionList(
int(self.subGoalNumber / 2) + 1
)
self.initOmega(newPosList)
self.subGoalPositionList[1: -1] += newPosList
self.filterSubGoal()
#print self.roadmap
retList = map(
lambda p: goal.CircleGoal(
subGoalRadius,
p,
self.screen
),
self.gPosList
)
retList[-1].radius = 1 * subGoalRadius
return retList
for p2 in range(1, distances.shape[0]):
distances[p1, p2] = dist(p1, p2)
existing_net = np.zeros((numpolygons + 1, numpolygons + 1))
# mark row 0 and column 0 as unused (there is no polygon #0)
existing_net[0] = np.nan
existing_net[::, 0] = np.nan
for (p1, p2, limit) in \
[(7, 4, 1100), (7, 16, 1000), (7, 11, 1100), (11, 17, 1100),
(16, 7, 400), (17, 11, 200), (11, 7, 200), (16, 17, 3500),
(16, 24, 1200), (17, 24, 250), (24, 16, 500), (24, 17, 100),
(24, 31, 900), (31, 24, 1100), (31, 36, 2000), (36, 31, 2000),
(36, 35, 1500), (35, 36, 2800), (33, 34, 100), (34, 33, 100),
(32, 27, 100), (27, 32, 550), (32, 33, 150), (33, 32, 230),
(32, 37, 900), (37, 32, 900), (37, 39, 900), (39, 37, 900),
(33, 39, 500), (39, 33, 1300), (34, 39, 1000), (39, 34, 1300),
(39, 38, 2500), (38, 39, 6400), (38, 41, 450), (41, 38, 600)]:
assert p1 in net[p2].keys(), (p2, p1)
assert p2 in net[p1].keys(), (p1, p2)
existing_net[p1, p2] = limit
connections = {}
for dest in range(1, numpolygons + 1):
for src in range(1, numpolygons + 1):
shortest = [n for n in dijkstra.shortestPath(net, src, dest)]
pairs = []
for i in range(len(shortest) - 1):
pairs.append((shortest[i], shortest[i + 1]))
connections[(src, dest)] = pairs
def processRouteRequest(src, destinations, junctionsDict, graphDict):
"""
- Transforms the source and destination coordinates to SUMO junctions ID
- Resolves the routing demand by using a dijkstra algorithm
- Sends the route (list of geographic coordinates) back to the client
"""
route = []
first = True
if isJunction(src):
#src become an edge predecessor of the src junction
src = iter(junctionsDict[getJunctionId(src)][0]).next()
srcJunction = True
else:
srcJunction = False
for dest in destinations:
if isJunction(dest):
#dest become an edge successor of the dest junction
dest = iter(junctionsDict[getJunctionId(dest)][1]).next()
destJunction = True
else:
destJunction = False
try:
#Getting shortest path
tmpRoute = dijkstra.shortestPath(graphDict, src, dest)
except Exception as e:
Logger.exception(e)
return constants.ROUTE_ERROR_CONNECTION, None
#Removing the first edge if it's the first routing and we find recurrence
if first and srcJunction and tmpRoute:
tmpRoute.pop(0)
elif first and len(tmpRoute) > 1 and tmpRoute[1] == getOppositeEdge(
tmpRoute[0]):
tmpRoute.pop(0)
#Removing the last edge if it was a junctions from start
if destJunction and tmpRoute:
tmpRoute.pop()
#Removing the first edge of routings with via points in order to avoid two identical edges when extending road
if not first:
tmpRoute.pop(0)
#Adding the calculated routing to the main routing
route.extend(tmpRoute)
first = False
#Updating source edge for the next routing
if tmpRoute:
src = tmpRoute[-1]
else:
src = dest
if len(route) > 1 and route[-2] == getOppositeEdge(route[-1]):
route.pop()
return 0, route
def spin(self):
# locate all markers on the object
if False:
max_angle = 60.0/180.0*math.pi
min_z = math.cos(max_angle)
print "min_z: %s"%(min_z)
# big_box
# marker_list = [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31]
# small_box
# marker_list = [32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45]
# table
# marker_list = [3,4,5,6]
# c-beam
marker_list = [46, 47, 48, 49, 50, 51, 52, 53, 54, 55]
mtrans_matrix = []
mtrans_weights_ori = []
mtrans_weights_pos = []
for i in range(0, len(marker_list)):
mtrans_matrix.append([])
mtrans_weights_ori.append([])
mtrans_weights_pos.append([])
for j in range(0, len(marker_list)):
if i > j:
mtrans_matrix[i].append([])
mtrans_weights_ori[i].append([])
mtrans_weights_pos[i].append([])
else:
mtrans_matrix[i].append(None)
mtrans_weights_ori[i].append(None)
mtrans_weights_pos[i].append(None)
marker_lock = Lock()
self.visible_markers = []
def markerCallback(data):
marker_lock.acquire()
self.visible_markers = []
for m in data.markers:
if m.id in marker_list:
self.visible_markers.append( (m.id, pm.fromMsg(m.pose.pose)) )
marker_lock.release()
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, markerCallback)
while not rospy.is_shutdown():
marker_lock.acquire()
visible_markers_local = copy.copy(self.visible_markers)
marker_lock.release()
accepted_markers = []
accepted_markers_id = []
accepted_markers_weights_pos = []
accepted_markers_weights_ori = []
for m in visible_markers_local:
n = PyKDL.Frame(m[1].M) * PyKDL.Vector(0,0,-1)
if n.z() > min_z:
accepted_markers.append( m )
accepted_markers_id.append( m[0] )
# n.z() is in range (min_z, 1.0), min_z is the best for orientation and 1.0 i best for position
weight_pos = (n.z() - min_z)/(1.0-min_z)
if weight_pos > 1.0 or weight_pos < 0.0:
print "error: weight==%s"%(weight_pos)
accepted_markers_weights_pos.append(weight_pos)
accepted_markers_weights_ori.append(1.0-weight_pos)
print "visible: %s accepted markers: %s"%(len(visible_markers_local), accepted_markers_id)
for i in range(0, len(accepted_markers)):
for j in range(i+1, len(accepted_markers)):
if accepted_markers[i][0] > accepted_markers[j][0]:
idx1 = i
idx2 = j
else:
idx1 = j
idx2 = i
# print " %s with %s"%(accepted_markers[idx1][0], accepted_markers[idx2][0])
T_C_M1 = accepted_markers[idx1][1]
T_C_M2 = accepted_markers[idx2][1]
T_M1_M2 = T_C_M1.Inverse() * T_C_M2
marker_id1_index = marker_list.index(accepted_markers[idx1][0])
marker_id2_index = marker_list.index(accepted_markers[idx2][0])
mtrans_matrix[marker_id1_index][marker_id2_index].append(T_M1_M2)
mtrans_weights_ori[marker_id1_index][marker_id2_index].append(accepted_markers_weights_ori[idx1] * accepted_markers_weights_ori[idx2])
mtrans_weights_pos[marker_id1_index][marker_id2_index].append(accepted_markers_weights_pos[idx1] * accepted_markers_weights_pos[idx2])
rospy.sleep(0.1)
G = {}
for i in range(0, len(marker_list)):
for j in range(0, len(marker_list)):
if mtrans_matrix[i][j] == None:
pass
# print "%s with %s: None"%(marker_list[i], marker_list[j])
elif len(mtrans_matrix[i][j]) < 10:
print "%s with %s: %s (ignoring)"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]))
else:
score, R_M1_M2 = velmautils.meanOrientation(mtrans_matrix[i][j], mtrans_weights_ori[i][j])
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).rot.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances, key=operator.itemgetter(0))
mtrans = []
weights_ori = []
for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]])
weights_ori.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]])
# score, R_M1_M2 = velmautils.meanOrientation(mtrans, weights_ori)
print "%s with %s: %s, score: %s"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]), score)
P_M1_M2 = velmautils.meanPosition(mtrans_matrix[i][j], mtrans_weights_pos[i][j])
print "P_M1_M2 before: %s"%(P_M1_M2)
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).vel.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances, key=operator.itemgetter(0))
mtrans = []
weights_pos = []
for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]])
weights_pos.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]])
# P_M1_M2 = velmautils.meanPosition(mtrans, weights_pos)
print "P_M1_M2 after: %s"%(P_M1_M2)
mtrans_matrix[i][j] = PyKDL.Frame(copy.deepcopy(R_M1_M2.M), P_M1_M2)
neighbours = G.get(marker_list[i], {})
if score > 0.01:
score = 1000000.0
else:
score = 1.0
neighbours[marker_list[j]] = score
G[marker_list[i]] = neighbours
neighbours = G.get(marker_list[j], {})
neighbours[marker_list[i]] = score
G[marker_list[j]] = neighbours
frames = []
# get the shortest paths weighted by score from optimization
for marker_id in marker_list:
path = dijkstra.shortestPath(G,marker_id,marker_list[0])
print path
T_Ml_Mf = PyKDL.Frame()
for i in range(0, len(path)-1):
if marker_list.index(path[i]) > marker_list.index(path[i+1]):
T_Mp_Mn = mtrans_matrix[marker_list.index(path[i])][marker_list.index(path[i+1])]
T_Ml_Mf = T_Ml_Mf * T_Mp_Mn
else:
T_Mn_Mp = mtrans_matrix[marker_list.index(path[i+1])][marker_list.index(path[i])]
T_Ml_Mf = T_Ml_Mf * T_Mn_Mp.Inverse()
frames.append(copy.deepcopy(T_Ml_Mf.Inverse()))
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
m_id = 0
marker_idx = 0
for fr in frames:
q = fr.M.GetQuaternion()
p = fr.p
print "[%s, PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s),PyKDL.Vector(%s,%s,%s))],"%(marker_list[marker_idx], q[0], q[1], q[2], q[3], p.x(), p.y(), p.z())
m_id = pub_marker.publishFrameMarker(fr, m_id, scale=0.1, frame='world', namespace='default')
rospy.sleep(0.1)
marker_idx += 1
rospy.sleep(2.0)
exit(0)
else:
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
markers2 = [
[46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))],
[47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0161417497092,-9.92379339669e-05,-0.00603105214296,0.999851519216),PyKDL.Vector(-0.0987990318312,-0.000265582834399,0.000544628226204))],
[48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706829255712,0.00114672638679,-0.707103949357,0.0198769487466),PyKDL.Vector(0.0427261427894,0.00245374838957,-0.116698579624))],
[49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706230029879,-0.00660144281521,-0.70769079068,0.0192174565616),PyKDL.Vector(0.0410352237403,0.000595788239244,-0.0336956438972))],
[50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0155276433453,0.714580229904,0.00743395758828,0.699341635824),PyKDL.Vector(-0.131991504795,-0.00410930997885,-0.0916799439467))],
[51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.706160148032,0.0114839861434,-0.707838133957,0.0130820300375),PyKDL.Vector(-0.150701418215,-0.000688426198715,-0.035762545196))],
[52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.705850422242,0.00260066465892,-0.708005925475,0.022271673869),PyKDL.Vector(-0.150663515172,-0.00196494029406,-0.123356224597))],
[53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00630495805624,-0.999979097775,0.000308879730173,0.00139860956497),PyKDL.Vector(-0.0116053782242,0.000352840524022,-0.0267278839783))],
[54, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00853722085061,-0.999853611966,0.00230495916657,0.0146477869582),PyKDL.Vector(-0.0949889134574,0.00131718330416,-0.0165548984986))],
[55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0208301706621,-0.711731154203,0.0052945617051,0.702123091589),PyKDL.Vector(0.0244779541548,0.000463479220587,-0.0804749548707))],
]
markers = [
[46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))],
[47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00211977224419,0.00133104595822,-0.00433238039783,0.999987482603),PyKDL.Vector(-0.0995553679408,-0.000651966932258,0.000444468330964))],
[48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.705539374795,0.00129956423061,-0.708394191186,0.0197527628665),PyKDL.Vector(0.0433256677966,0.00212664651843,-0.116482343501))],
[49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.704707757517,-0.00628228374428,-0.709255128279,0.0174548680028),PyKDL.Vector(0.0412709849952,0.000494665961165,-0.0338667341872))],
[50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0117276773848,0.706312439798,0.00558379104701,0.707781053891),PyKDL.Vector(-0.131246837996,-0.00469319026484,-0.0943814089463))],
[51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.710112841604,0.00963407546503,-0.703895468304,0.013345653983),PyKDL.Vector(-0.149984963191,-0.00300459973807,-0.0370193446712))],
[52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.704691425649,0.00497982940017,-0.709159595229,0.0218601100464),PyKDL.Vector(-0.15277553848,-0.00431480401095,-0.120995842686))],
[53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00984066000086,-0.999945658681,0.00299307949816,0.00169781765667),PyKDL.Vector(-0.0132269965227,-0.00110379001368,-0.0274175040768))],
[54, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00154140261629,0.999633307109,-0.00751679824708,0.0259686953912),PyKDL.Vector(-0.0974091549673,-0.000670004842722,-0.0319416169884))],
[55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0195903374145,-0.713404165076,0.00273342374532,0.700473585745),PyKDL.Vector(0.0250633176495,-0.00356911439713,-0.0811403242495))],
]
m_id = 0
for m in markers:
print m[0]
print m[1]
m_id = pub_marker.publishFrameMarker(m[1], m_id, scale=0.1, frame='world', namespace='default')
rospy.sleep(0.1)
def main(startLatitude, startLongitude, endLatitude, endLongitude, maxDistance,
supportedChargers):
print("Main Start")
requestList = requestGenerator(startLatitude, startLongitude, endLatitude,
endLongitude, supportedChargers)
print("generating graph")
graph = graphGenerator(requestList, maxDistance)
#shortestPath = dijkstra.shortestPath(graph,"start","end")
#actualDistances = googleNodes.googleMapsActualPath(requestList, shortestPath)
correctPath = False
#print(shortestPath)
count = 0
print("searching for best path")
while correctPath == False:
correctPath = True
currentDistanceCounter = 0
print("calculating path")
shortestPath = dijkstra.shortestPath(graph, "start", "end")
#print("shortestPath {}".format(shortestPath))
if shortestPath == None or None in shortestPath or count > 50:
if count > 50:
print("too many trimming iterations")
print(shortestPath)
return None
break
else:
actualDistances = googleNodes.googleMapsActualPath(
requestList, shortestPath)
print("actual distances of the way points in KM are {}".format(
actualDistances))
print("verifying path")
while currentDistanceCounter < len(actualDistances):
if actualDistances[currentDistanceCounter] / 1000 > maxDistance:
correctPath = False
graph[str(shortestPath[currentDistanceCounter])].pop(
str(shortestPath[currentDistanceCounter + 1]), None)
else:
graph[str(shortestPath[currentDistanceCounter])][str(
shortestPath[
currentDistanceCounter +
1])] = actualDistances[currentDistanceCounter] / 1000
currentDistanceCounter += 1
# if(correctPath==True):
# break
#print(graph)
print("actual distances {}".format(actualDistances))
count += 1
if shortestPath == None:
print("No path found")
return None
print("Packaging results")
waypointsData = []
waypointsData.append([requestList[0][0], requestList[0][1], None, None])
for point in shortestPath:
if point != "start" and point != "end":
waypointsData.append([
requestList[int(point) + 1][0], requestList[int(point) + 1][1],
requestList[int(point) + 1][3], requestList[int(point) + 1][4]
])
#print(requestList[int(point)+1])
waypointsData.append([requestList[1][0], requestList[1][1], None, None])
#print(waypoints)
waypoints = {}
waypoints['Waypoints'] = waypointsData
return waypoints
#current Inputs
#startLatitude = 39.5
#startLongitude = -121.9
#endLatitude = 40
#endLongitude = -121
#maxDistance = 100
#supportedChargers = []
#waypoints = main(startLatitude,startLongitude,endLatitude,endLongitude,maxDistance,supportedChargers)
#print(waypoints)
#https://maps.googleapis.com/maps/api/distancematrix/json?units=imperial&origins=longtitude,latitude&destinations=40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.6905615%2C-73.9976592%7C40.659569%2C-73.933783%7C40.729029%2C-73.851524%7C40.6860072%2C-73.6334271%7C40.598566%2C-73.7527626%7C40.659569%2C-73.933783%7C40.729029%2C-73.851524%7C40.6860072%2C-73.6334271%7C40.598566%2C-73.7527626&key=YOUR_API_KEY
def shortest_path(board, start, end, test=is_nonwall):
"Return the shortest path between two points on the board."
return dijkstra.shortestPath(DijkstraGraph(board, test), start, end)
