def nodeToList(n):
#if n == None:
#print("ntl:", n)
b1 = heap.GetID(n)
#print("1", b1)
b2 = heap.GetDependents(n)
#print("2", b2)
b3 = heap.GetStep(n)
#print("3", b3)
b4 = heap.GetDecision(n)
#print("4", b4)
b5 = heap.GetParent(n)
#print("5", b5)
b6 = float(heap.GetTime(n))
#print("6", b6)
b7 = float(heap.GetVelocity(n))
#print("7", b7)
b8 = (heap.GetGear(n), heap.GetDelta(n), heap.GetTimeToShift(n))
#print("8", b8)
b9 = float(heap.GetScore(n))
#print("9", b9)
b = [b1, b2, b3, b4, b5, b6, b7, b8, b9]
#print(b)
return b
def find_optimum(self, endStates):
min_t = self.maxint
min_S = None
for statePointer in endStates:
if heap.GetTime(statePointer) < min_t:
min_t = heap.GetTime(statePointer)
min_S = statePointer
path = []
output = np.zeros([self.n, O_MATRIX_COLS])
output[0, :] = np.array([
heap.GetTime(min_S),
heap.GetStep(min_S),
heap.GetVelocity(min_S), 0, 0, 0,
heap.GetDecision(min_S),
heap.GetGear(min_S), 0, 0, 0, 0,
self.segments[heap.GetStep(min_S) - 1].curvature, 0, 0
])
i = self.n - 1
while heap.GetParent(min_S) != None:
print(nodeToList(min_S))
min_S = heap.GetParent(min_S)
print(min_S)
path.append(heap.GetDecision(min_S))
output[i, :] = np.array([
heap.GetTime(min_S),
heap.GetStep(min_S),
heap.GetVelocity(min_S), 0, 0, 0,
heap.GetDecision(min_S),
heap.GetGear(min_S), 0, 0, 0, 0,
self.segments[heap.GetStep(min_S) - 1].curvature, 0, 0
])
i = i - 1
print(output)
return (path[::-1], output)
def nodeToList(n):
return [
heap.GetID(n),
heap.GetDependents(n),
heap.GetStep(n),
heap.GetDecision(n),
heap.GetParent(n),
float(heap.GetTime(n)),
float(heap.GetVelocity(n)),
(heap.GetGear(n), heap.GetDelta(n), heap.GetTimeToShift(n)),
float(heap.GetScore(n))
]
def printNode(n):
print("ID: ", heap.GetID(n))
print("Dependents: ", heap.GetDependents(n))
print("Step: ", heap.GetStep(n))
print("Decision: ", GetDecision(n))
print("Parent: ", heap.GetParent(n))
print("Time: ", heap.GetTime(n))
print("Velocity: ", heap.GetVelocity(n))
print("Gear: ", heap.GetGear(n))
print("Delta: ", heap.GetDelta(n))
print("Time to shift: ", heap.GetTimeToShift(n))
print("Score: ", heap.GetScore(n))
def makeChildren(self, num):
index = self.flatToList(num)
children = []
for i in range(self.axes):
if index[i] < self.n - 1:
child = list(index)
child[i] = index[i] + 1
c = self.listToFlat(child)
if self.getNode(c) == None:
children.append(c)
childPointers = []
newStep = heap.GetStep(self.nodes[num]) + 1
for child in children:
if child not in self.nodes:
n = heap.MakeInitNode()
heap.SetID(n, child)
heap.SetStep(n, newStep)
self.addNode(child, n)
childPointers.append(n)
return childPointers
def find_optimum(self, endStates):
for k in self.rk.nodes.keys():
if self.rk.getNode(k) != None and heap.GetID(
self.rk.getNode(k)) != k:
print("uh oh:", k)
for e in endStates:
print(e, self.rk.flatToList(e), heap.GetTime(self.rk.getNode(e)))
if len(endStates) <= 0:
print("did not succeed")
exit()
min_t = self.maxint
min_S = None
for s in endStates:
statePointer = self.rk.getNode(s)
#if statePointer == None:
#print("fo:", statePointer)
if heap.GetTime(statePointer) < min_t:
min_t = heap.GetTime(statePointer)
min_S = statePointer
#min_S = self.rk.getNode(1931)
data = nodeToList(min_S)
print(data)
path = []
output = np.zeros([self.n, O_MATRIX_COLS])
#if min_S == None:
#print("foo:", min_S)
output[self.n - 1, :] = np.array([
heap.GetTime(min_S),
heap.GetStep(min_S),
heap.GetVelocity(min_S), 0, 0, 0,
heap.GetDecision(min_S),
heap.GetGear(min_S), 0, 0, 0, 0,
self.segments[heap.GetStep(min_S) - 1].curvature, 0, 0
])
i = self.n - 2
while data[4] >= 0:
min_S = self.rk.getNode(data[4])
print("##########################################")
data = nodeToList(min_S)
print(data)
path.append(heap.GetDecision(min_S))
output[i, :] = np.array([
heap.GetTime(min_S),
heap.GetStep(min_S),
heap.GetVelocity(min_S), 0, 0, 0,
heap.GetDecision(min_S),
heap.GetGear(min_S), 0, 0, 0, 0,
self.segments[heap.GetStep(min_S) - 1].curvature, 0, 0
])
i = i - 1
print(output[:, O_STATUS])
print(output[:, O_VELOCITY])
print(output[:, O_CURVATURE])
return (path[::-1], output)