def __init__(self):
self.aircraftList = []
self.aircraftList.append(
AStar.Aircraft("DJI Mavic Air", 19, 4, 0.43, 27))
self.aircraftList.append(AStar.Aircraft("DIY Drone", 12, 4, 0.3, 27))
self.aircraftList.append(
AStar.Aircraft("DJI Phantom 4", 20, 3, 1.375, 1))
self.aircraftList.append(
AStar.Aircraft("DJI Matrice 600 Pro", 18, 5, 10, 1))
def __init__(self, matrix, traffic, duration=3600, dynamicObstacles=None):
super(AStarWithHolding, self).__init__(matrix, traffic, duration,
dynamicObstacles)
# plan trajectory
self.traffic = traffic
self.initPlanner = AStar.AStarMultiple(self.matrix,
self.traffic.trafficPlan)
self.planResult = list()
self.duration = duration
def MultiSearch(self):
# ct = 1
for flight in self.trafficPlan.scheduledFlights:
pathFinder = Dij(self.matrix, flight.startPoint, flight.endPoint,
flight.aircraft)
trajectory = AStar.Trajectory(self.matrix, pathFinder.Search(),
flight.aircraft,
flight.departureTime)
self.planResult.append(trajectory)
def __init__(self,
matrix,
aircraftBase,
noFlights=20,
earliestDepartureTime=0,
latestDepartureTime=300):
self.trafficPlan = TrafficPlan.TrafficPlan()
startPoints = list()
for j in range(matrix.indexRange[0]):
for k in range(matrix.indexRange[1]):
l = 0
while True:
index = int(j + k * matrix.indexRange[0] + l *
matrix.indexRange[0] * matrix.indexRange[1])
if matrix.indexAvailable[index]:
startPoints.append(AStar.Point((j, k, l)))
break
l = l + 1
if l == matrix.indexRange[2]:
break
# for l in range(matrix.indexRange[2]):
# index = int(j + k * matrix.indexRange[0] + l * matrix.indexRange[0] * matrix.indexRange[1])
# if matrix.indexAvailable[index]:
# startPoints.append(AStar.Point((j, k, l)))
# for j in range(matrix.indexRange[0]):
# for k in range(matrix.indexRange[1]):
# index = int(j + k * matrix.indexRange[0] + l * matrix.indexRange[0] * matrix.indexRange[1])
# if matrix.indexAvailable[index]:
# startPoints.append(AStar.Point((j,k,0)))
self.odPairs = random.sample(startPoints, 2 * noFlights)
for i in range(noFlights):
startPoint = self.odPairs[i * 2]
endPoint = self.odPairs[i * 2 + 1]
# startPoint = AStar.Point((random.randint(0, matrix.indexRange[0]-1),
# random.randint(0, matrix.indexRange[1]-1), 0))
# endPoint = AStar.Point((random.randint(0, matrix.indexRange[0]-1),
# random.randint(0, matrix.indexRange[1]-1), 0))
departureTime = random.randint(earliestDepartureTime,
latestDepartureTime)
aircraft = aircraftBase.aircraftList[random.randint(
0,
len(aircraftBase.aircraftList) - 1)]
plan = TrafficPlan.FlightPlan(startPoint, endPoint, departureTime,
aircraft)
self.trafficPlan.AppendFlightPlan(plan)
def Search(self):
startNode = AStar.Node(self.startPoint, self.endPoint,
self.matrix.cellLength, self.matrix.cellHeight,
self.aircraft.speed)
startNode.timeEntering = self.departureTime
self.openList.append(startNode)
while True:
# find minF
minF = self.GetMinNode()
# minF to closeList
self.closeList.append(minF)
self.openList.remove(minF)
neighbours = self.matrix.FindInNetwork(
int(minF.point.x + minF.point.y * self.matrix.indexRange[0] +
minF.point.z * self.matrix.indexRange[0] *
self.matrix.indexRange[1]))
for nextIndex in neighbours:
holding = None
neighbourPoint = AStar.Point(
(self.matrix.FindInNodelist(nextIndex[0]).x,
self.matrix.FindInNodelist(nextIndex[0]).y,
self.matrix.FindInNodelist(nextIndex[0]).z))
# avoid conflicting with the other OD pairs
# if not (neighbourPoint == self.endPoint or neighbourPoint == self.startPoint):
# if neighbourPoint in self.odPairs:
# continue
if self.PointInCloseList(
neighbourPoint): # ignore if in close list
continue
timeEntering = int(self.departureTime + minF.g + nextIndex[1] /
self.aircraft.speed[nextIndex[2]] / 2)
# neighbourPoint.timeEntering = timeEntering
timeExiting = int(timeEntering +
max((np.sqrt(self.matrix.cellLength**2 * 2 +
self.matrix.cellHeight**2) /
self.aircraft.speed[3]),
(np.sqrt(self.matrix.cellLength**2 +
self.matrix.cellHeight**2) /
self.aircraft.speed[2])))
if not sum(self.dynamicObstacles.dynamicObsList[
nextIndex[0], timeEntering:timeExiting]
) == 0: # if meet an occupied block
for requiredHoldingTime in range(900):
if sum(self.dynamicObstacles.dynamicObsList[
nextIndex[0],
(timeEntering + requiredHoldingTime):(
timeExiting + requiredHoldingTime
)]) == 0: # required minimum holding time
holdingTime = requiredHoldingTime
continue
if holdingTime == 899: # max holding time at each node
continue
timeEnteringMinF = minF.timeEntering
timeExitingMinF = timeEntering + holdingTime
if not sum(self.dynamicObstacles.dynamicObsList[
nextIndex[0], timeEnteringMinF:timeExitingMinF]
) == 0: # if holding not available
continue
# if holding available
nextNode = AStar.Node(neighbourPoint, self.endPoint,
self.matrix.cellLength,
self.matrix.cellHeight,
self.aircraft.speed)
nextNode.g = minF.g + nextIndex[1] / self.aircraft.speed[
nextIndex[2]] + holdingTime
nextNode.timeEntering = timeExitingMinF
nextNode.afterHolding = True
nextNode.holdingTime = holdingTime
existNode = self.PointInOpenList(neighbourPoint)
if not existNode:
nextNode.father = minF
self.openList.append(nextNode)
continue
if nextNode.g < existNode.g:
existNode.g = nextNode.g
existNode.timeEntering = nextNode.timeEntering
existNode.afterHolding = True
existNode.holdingTime = nextNode.holdingTime
existNode.father = minF
else: # if not meet an occupied block
nextNode = AStar.Node(neighbourPoint, self.endPoint,
self.matrix.cellLength,
self.matrix.cellHeight,
self.aircraft.speed)
nextNode.g = minF.g + nextIndex[1] / self.aircraft.speed[
nextIndex[2]]
existNode = self.PointInOpenList(
neighbourPoint
) # return if the node exist in open list
if not existNode:
nextNode.father = minF
self.openList.append(nextNode)
continue
if nextNode.g < existNode.g: # set g and father if the node exist in open list
existNode.g = nextNode.g
existNode.father = minF
lastNode = self.EndPointInCloseList()
if lastNode:
pathList = []
while True:
if lastNode.father:
pathList.append(lastNode)
lastNode = lastNode.father
else:
pathList.append(startNode)
# return list(reversed(pathList))
return AStar.Trajectory(self.matrix,
list(reversed(pathList)),
self.aircraft,
self.departureTime)
if len(self.openList) == 0:
return None
def Search(self):
startNode = AStar.Node(self.startPoint, self.endPoint,
self.matrix.cellLength, self.matrix.cellHeight,
self.aircraft.speed)
self.openList.append(startNode)
while True:
# find minF
minF = self.GetMinNode()
# minF to closeList
self.closeList.append(minF)
self.openList.remove(minF)
neighbours = self.matrix.FindInNetwork(
int(minF.point.x + minF.point.y * self.matrix.indexRange[0] +
minF.point.z * self.matrix.indexRange[0] *
self.matrix.indexRange[1]))
for nextIndex in neighbours:
neighbourPoint = AStar.Point(
(self.matrix.FindInNodelist(nextIndex[0]).x,
self.matrix.FindInNodelist(nextIndex[0]).y,
self.matrix.FindInNodelist(nextIndex[0]).z))
# avoid conflicting with the other OD pairs
if not (neighbourPoint == self.endPoint
or neighbourPoint == self.startPoint):
if neighbourPoint in self.odPairs:
continue
timeEntering = int(self.departureTime + minF.g + nextIndex[1] /
self.aircraft.speed[nextIndex[2]] / 2)
timeExiting = int(timeEntering +
max((np.sqrt(self.matrix.cellLength**2 * 2 +
self.matrix.cellHeight**2) /
self.aircraft.speed[3]),
(np.sqrt(self.matrix.cellLength**2 +
self.matrix.cellHeight**2) /
self.aircraft.speed[3])))
if not sum(self.dynamicObstacles.dynamicObsList[
nextIndex[0], timeEntering:timeExiting]) == 0:
continue
currentPoint = AStar.Point(
(self.matrix.FindInNodelist(nextIndex[0]).x,
self.matrix.FindInNodelist(nextIndex[0]).y,
self.matrix.FindInNodelist(nextIndex[0]).z))
if self.PointInCloseList(
currentPoint): # ignore if in close list
continue
nextNode = AStar.Node(currentPoint, self.endPoint,
self.matrix.cellLength,
self.matrix.cellHeight,
self.aircraft.speed)
nextNode.g = minF.g + nextIndex[1] / self.aircraft.speed[
nextIndex[2]]
existNode = self.PointInOpenList(
currentPoint) # return if the node exist in open list
if not existNode:
nextNode.father = minF
self.openList.append(nextNode)
continue
if nextNode.g < existNode.g: # set g and father if the node exist in open list
existNode.g = nextNode.g
existNode.father = minF
lastNode = self.EndPointInCloseList()
if lastNode: # if endPoint is in close list, return result
pathList = []
while True:
if lastNode.father:
pathList.append(lastNode)
lastNode = lastNode.father
else:
# return list(reversed(pathList))
return AStar.Trajectory(self.matrix,
list(reversed(pathList)),
self.aircraft,
self.departureTime)
if len(self.openList) == 0:
return "None"
aircraftList = RandomAircraftCreator.AircraftDataBase()
airmatrix = MatrixBuilder.Matrix((3000, 3000, 2000), 100, 50)
airmatrix.NodeListConstructor()
airmatrix.MatrixConstructor()
traffic = TrafficGenerator.TrafficGenerator(airmatrix, aircraftList, 100)
# Dijkstra Algorithm
finder = Dijkstra.MultiDij(airmatrix, traffic.trafficPlan)
time_start = time.time()
finder.MultiSearch()
dijkstra_result = finder.planResult
time_end = time.time()
print('time cost', time_end-time_start, 's')
# AStar Algorithm
finder = AStar.AStarMultiple(airmatrix, traffic.trafficPlan)
time_start = time.time()
finder.MultiSearch()
astar_result = finder.planResult
time_end = time.time()
print('time cost', time_end-time_start, 's')
for i in range(100):
print("\nTrajectory "+str(i+1)+":")
dij_start_time = dijkstra_result[i].trajectory[0][2]
dij_length = dijkstra_result[i].trajectory[0]
dij_end_time = dijkstra_result[i].trajectory[len(dijkstra_result[i].trajectory)-1][2]
dij_cost = dij_end_time - dij_start_time
print("Dijstra cost: "+str(dij_cost)+" seconds")
astar_start_time = astar_result[i].trajectory[0][2]
def Search(self):
# add startPoint to openList
# startNode = AStar.Node(self.startPoint, self.endPoint, self.gainHorizontal, self.gainVertical)
startNode = AStar.Node(self.startPoint, self.endPoint,
self.matrix.cellLength, self.matrix.cellHeight,
self.aircraft.speed)
self.openList.append(startNode)
# search
while True:
# find minF
minF = self.GetMinNode()
# minF to closeList
self.closeList.append(minF)
self.openList.remove(minF)
# test the neighbour of minF
# neighbours = self.matrix.network[minF.index]
neighbours = self.matrix.FindInNetwork(
int(minF.point.x + minF.point.y * self.matrix.indexRange[0] +
minF.point.z * self.matrix.indexRange[0] *
self.matrix.indexRange[1]))
for nextIndex in neighbours:
neighbourPoint = AStar.Point(
(self.matrix.FindInNodelist(nextIndex[0]).x,
self.matrix.FindInNodelist(nextIndex[0]).y,
self.matrix.FindInNodelist(nextIndex[0]).z))
if not (neighbourPoint == self.endPoint
or neighbourPoint == self.startPoint):
if neighbourPoint in self.odPairs:
continue
if not sum(self.activeObstacles[nextIndex[0], :]) == 0:
continue
currentPoint = AStar.Point(
(self.matrix.FindInNodelist(nextIndex[0]).x,
self.matrix.FindInNodelist(nextIndex[0]).y,
self.matrix.FindInNodelist(nextIndex[0]).z))
if self.PointInCloseList(
currentPoint): # ignore if in close list
continue
# nextNode = AStar.Node(currentPoint, self.endPoint, self.gainHorizontal, self.gainVertical)
nextNode = AStar.Node(currentPoint, self.endPoint,
self.matrix.cellLength,
self.matrix.cellHeight,
self.aircraft.speed)
nextNode.g = minF.g + nextIndex[1] / self.aircraft.speed[
nextIndex[2]]
existNode = self.PointInOpenList(
currentPoint) # return if the node exist in open list
if not existNode:
nextNode.father = minF
self.openList.append(nextNode)
continue
if nextNode.g < existNode.g: # set g and father if the node exist in open list
existNode.g = nextNode.g
existNode.father = minF
lastNode = self.EndPointInCloseList()
if lastNode: # if endPoint is in close list, return result
pathList = []
while True:
if lastNode.father:
pathList.append(lastNode)
lastNode = lastNode.father
else:
# return list(reversed(pathList))
return AStar.Trajectory(self.matrix,
list(reversed(pathList)),
self.aircraft, self.startTime)
if len(self.openList) == 0:
return "None"
obstacle.append(Point(i, 600))
MyMap.add_obstacles(obstacle)
#MyMap.plot_map(sx, sy, gx, gy)
# initial state
#path Creation
#AStar parameters
grid_size = 70 # [mm] #précision du A*
robot_size = 150 # [mm] #distance de sécurité avec les obstacles i.e. taille du robot/2
#AStar planning
ox, oy = MyMap.get()
rx, ry = AStar.a_star_planning(sx, sy, gx, gy, ox, oy, grid_size,
robot_size)
#calcul controlPoints used for Bezier
controlPoints = AStar.calc_control_points(rx, ry, sx, sy, gx, gy)
NPoints = 100 # number of points(for Bezier) between each control points calculated by Astar
#Bezier
Path_truc = Bezier.calc_bezier_path(controlPoints,
n_points=NPoints * len(controlPoints))
truc = []
for point in Path_truc:
truc.append(Point(point[0], point[1]))
#calcul cx,cy use for Pure Poursuit
#cx,cy = Bezier.calc_cxcy(Path)
print(robot)
comm = main_communication.CommManager(robot)
behaviour = state_machine.FSMMatch(robot)
comm.sendPositionMessage()
comm.sendLidarMessage(1, 0, 0, 0, 0)
# start and goal position EN MILIMETRE (Meme unite que le robot) !!!!
sx = 300.0 # [mm]
sy = 450.0 # [mm]
gx = 1300.0 # [mm]
gy = 450.0 # [mm]
grid_size = 70 # [mm] #précision du A*
robot_size = 150 # [mm] #distance de sécurité avec les obstacles i.e. taille du robot/2
print("A* BEGIN")
ox, oy = AStar.create_map()
#obstacle = [Point(600,1500),Point(700,1500),Point(800,1500),Point(900,1500)]
#AStar.add_obstacles(ox, oy, obstacle)
rx, ry = AStar.a_star_planning(sx, sy, gx, gy, ox, oy, grid_size,
robot_size)
print("A* DONE")
print("Bezier BEGIN")
control_points = [Point(rx[i], ry[i]) for i in range(len(rx) - 1, -1, -1)]
poly = Polygon(AStar.BordDeMap())
#on repère les points critique où il faudra passer nécessairement (points proche des murs)
#puis on calcul les chemins avec Bezier entre ces différents portions de trajectoire
AllPath = []
newPath = []
for i, point in enumerate(control_points):
newPath.append(point)
result = pd.DataFrame()
time_accu_d = 0
time_accu_a = 0
time_accu_4d = 0
delay_accu_4d = 0
result_d = []
result_a = []
result_4d = []
for flight in traffic.trafficPlan.scheduledFlights:
time_start = time.time()
finder_d = Dijkstra.Dij(airmatrix, flight.startPoint, flight.endPoint, flight.aircraft)
trajectory_d = AStar.Trajectory(airmatrix, finder_d.Search(), flight.aircraft, flight.departureTime)
time_end = time.time()
time_d = time_end - time_start
time_accu_d += time_d
result_d.append(trajectory_d)
aobt_d = trajectory_d.trajectory[0][2]
aibt_d = trajectory_d.trajectory[len(trajectory_d.trajectory) - 1][2]
cost_d = aibt_d - aobt_d
time_start = time.time()
finder_a = AStar.AStarClassic(airmatrix, flight.startPoint, flight.endPoint, flight.aircraft)
trajectory_a = AStar.Trajectory(airmatrix, finder_a.Search(), flight.aircraft, flight.departureTime)
time_end = time.time()
time_a = time_end - time_start
time_accu_a += time_a
def main():
#state initialisation
#map creation
# -Map
# -add obstacle
#goal and start point definition
#path creation
#- AStar planning (+ AStar parameters grid size and robot size)
#- Bezier
#Pure-Poursuite
#Map
MyMap = Map.Map()
obstacle = [] #rectangle x = 700 - 800 y = 300 - 600
for i in range(300, 600, 10):
obstacle.append(Point(700, i))
for i in range(300, 600, 10):
obstacle.append(Point(800, i))
for i in range(700, 800, 10):
obstacle.append(Point(i, 300))
for i in range(700, 800, 10):
obstacle.append(Point(i, 600))
MyMap.add_obstacles(obstacle)
#goal and start
sx = 300.0 # [mm]
sy = 450.0 # [mm]
gx = 1300.0 # [mm]
gy = 450.0 # [mm]
#MyMap.plot_map(sx, sy, gx, gy)
# initial state
state = TestPP.State(x=sx, y=sy, yaw=-math.pi / 2, v=0.0)
#path Creation
#AStar parameters
grid_size = 70 # [mm] #précision du A*
robot_size = 150 # [mm] #distance de sécurité avec les obstacles i.e. taille du robot/2
#AStar planning
ox, oy = MyMap.get()
rx, ry = AStar.a_star_planning(sx, sy, gx, gy, ox, oy, grid_size,
robot_size)
#calcul controlPoints used for Bezier
controlPoints = AStar.calc_control_points(rx, ry, sx, sy, gx, gy)
NPoints = 10 # number of points(for Bezier) between each control points calculated by Astar
#Bezier
Path = Bezier.calc_bezier_path(controlPoints,
n_points=NPoints * len(controlPoints))
#calcul cx,cy use for Pure Poursuit
cx, cy = Bezier.calc_cxcy(Path)
#parameter for Pure-Poursuite
k = 0.1 # look forward gain
Lfc = 2.0 # look-ahead distance
Kp = 1.0 # speed proportional gain
dt = 0.1 # [s] time increment for simulation
L = 2.9 # [mm] wheel base of vehicle
target_speed = 300.0 / 3.6 # [mm/s]
T = 100 #simulation time
#initialisation
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = TestPP.calc_target_index(state, cx, cy)
speed_cons = 0
while T >= time and lastIndex > target_ind:
ai = TestPP.PIDControl(target_speed, state.v)
di, target_ind = TestPP.pure_pursuit_control(state, cx, cy, target_ind)
speed_cons += ai
print(speed_cons, di * 180 / math.pi)
state = TestPP.update(state, ai, di)
time = time + dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if True: # pragma: no cover
plt.cla()
MyMap.plot_map(sx, sy, gx, gy)
TestPP.plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)