def __findOptimizedChargingPoint(self, start, end, lakeList):
nearestPoint = start
nearestDist = 99999
if len(lakeList) != 0:
for lake in lakeList:
for landingPoint in lake.landingList:
landingCoordinate = landingPoint.gpsLandingCoordinate
distEnd = distBetweenCoord(landingCoordinate[0],
landingCoordinate[1],
end.get_x(), end.get_y())
# print('distEnd', distEnd)
landingMissionPoint = MissionItem(
build_simple_mission_item(landingCoordinate[0],
landingCoordinate[1],
"charging"))
landingMissionPoint.setID('charging')
landingMissionPoint.setLandingPoint(landingPoint)
timeToStart = self.getTimeToFly(landingMissionPoint, start)
if timeToStart < self.__currentAutonomy and distEnd < nearestDist:
# print('nearestDist', nearestDist)
nearestPoint = landingMissionPoint
nearestDist = distEnd
# nearestPoint = landingMissionPoint
return nearestPoint
def node_create(self, lakeList, missionItemStart, missionItemEnd,
distanceMax):
allNodes = {}
# End landing point
missionItemEnd.setID('end')
allNodes['end'] = missionItemEnd
# Start landing point
missionItemStart.setID('start')
missionItemStart.setDistanceEnd(
self.getTimeToFly(missionItemStart, missionItemEnd))
allNodes['start'] = missionItemStart
for x in range(0, len(lakeList)):
gpsLandingPoints = lakeList[x].getSortLandingPoint(
self.droneSpeed * (distanceMax / 60))
for y in range(0, len(gpsLandingPoints)):
gpsLanding = gpsLandingPoints[y].gpsLandingCoordinate
missionItem = MissionItem(
build_simple_mission_item(
gpsLanding[0], gpsLanding[1],
'lac_id:' + str(x) + ':' + str(y)))
missionItem.setID('lac_id:' + str(x) + ':' + str(y))
missionItem.setLandingPoint(gpsLandingPoints[y])
missionItem.setDistanceEnd(
self.getTimeToFly(missionItem, missionItemEnd))
allNodes['lac_id:' + str(x) + ':' + str(y)] = missionItem
return allNodes
def __addMissionItem(self, missionItem):
if missionItem.getID() == 'charging':
newItemSleep = MissionItem(
build_simple_mission_item(missionItem.get_x(),
missionItem.get_x(), 'sleep'))
newItemSleep.setID('sleep')
takeoffCoordinate = missionItem.landingPoint.gpsDeriveCoordinate
newItemTakeOff = MissionItem(
build_simple_mission_item(takeoffCoordinate[0],
takeoffCoordinate[1], 'takeoff'))
newItemTakeOff.setID('takeoff')
self.__setTimeSpentInMission(missionItem)
self.finalMissionItemList.append(missionItem)
self.__setTimeSpentInMission(newItemSleep)
self.finalMissionItemList.append(newItemSleep)
self.finalMissionItemList.append(newItemTakeOff)
self.resultingWay.append([
missionItem.get_x(),
missionItem.get_y(),
missionItem.get_name()
])
self.resultingWay.append([
newItemSleep.get_x(),
newItemSleep.get_y(),
newItemSleep.get_name()
])
self.resultingWay.append([
newItemTakeOff.get_x(),
newItemTakeOff.get_y(),
newItemTakeOff.get_name()
])
else:
self.__setTimeSpentInMission(missionItem)
self.finalMissionItemList.append(missionItem)
self.resultingWay.append([
missionItem.get_x(),
missionItem.get_y(),
missionItem.get_name()
])
def node_create(lakeList, missionItemStart, missionItemEnd, distanceMax):
allNodes = {}
# End landing point
missionItemEnd.setID('end')
allNodes['end'] = missionItemEnd
# Start landing point
missionItemStart.setID('start')
missionItemStart.setDistanceEnd(missionItemStart.distanceTo(lat=missionItemEnd.get_x(), long=missionItemEnd.get_y()))
allNodes['start'] = missionItemStart
for x in range(0, len(lakeList)):
# gpsLandingPoints = lakeList[x].getLandingPoint()
gpsLandingPoints = lakeList[x].getSortLandingPoint(distanceMax)
for y in range(0, len(gpsLandingPoints)):
missionItem = MissionItem(build_simple_mission_item(gpsLandingPoints[y][0], gpsLandingPoints[y][1],'lac_id:' + str(x) + ':' + str(y)))
missionItem.setID('lac_id:' + str(x) + ':' + str(y))
missionItem.setDistanceEnd(
missionItem.distanceTo(lat=missionItemEnd.get_x(), long=missionItemEnd.get_y()))
allNodes['lac_id:' + str(x) + ':' + str(y)] = missionItem
return allNodes
def run(self):
print('=====================================')
#Plan a mission between each pairedPoint
pairedMissionPoint = self.__getPairedMissionPoints()
#Add the starting missionPoint
self.__addMissionItem(pairedMissionPoint[0][0])
#Get all the landing point
lakeList = self.__getTotalLakeList(self.maximalMapPoint[0],
self.maximalMapPoint[1],
self.maximalMapPoint[2],
self.maximalMapPoint[3])
for pairedPoint in pairedMissionPoint:
#Get the time needed to get to the next mission point
timeToFlyBetweenPoints = self.getTimeToFly(pairedPoint[0],
pairedPoint[1])
#If we can't go directly to the next point
if timeToFlyBetweenPoints > self.__currentAutonomy:
startPoint = pairedPoint[0]
if self.__currentAutonomy < self.__timeAutonomy:
#Find the optimize charging points
chargingPoint = self.__findOptimizedChargingPoint(
pairedPoint[0], pairedPoint[1], lakeList)
self.__addMissionItem(chargingPoint)
self.__resetAutonomy()
#After the charging, execute aStar from the charging point
startPoint = MissionItem(
build_simple_mission_item(chargingPoint.get_x(),
chargingPoint.get_y(),
"start"))
#Really important to set the ID to start because it's use in the A* algorithm
startPoint.setID('start')
#Once we have every landing points, the start point and end point we can generate a graph for A* and run it
success, result = self.__runAStar(startPoint, pairedPoint[1],
lakeList)
if success:
self.__compileAStarResult(result, lakeList)
else:
logger.error(
"The last mission point wasn't reach... So the mission is probably impossible"
)
sys.exit(
"ERROR: The last mission point wasn't reach. The mission is probably impossible."
)
else:
self.__setAutonomy(timeToFlyBetweenPoints)
#Add directly the end point
pairedPoint[1].setID('end')
self.__addMissionItem(pairedPoint[1])
#self.missionPlan.mission.set_missionitems2(self.finalMissionItemList)
self.__missionIndex = self.__missionIndex + 1
#Use to debug when the mission is not completly done
partialMissionFileName = self.exportPath + 'partialMission_' + str(
self.__missionIndex) + '.csv'
#with open(partialMissionFileName, 'w') as f:
# writer = csv.writer(f)
# writer.writerows(self.resultingWay)
self.missionPlan.mission.set_missionitems(self.finalMissionItemList)
with open(self.exportPath + 'completeMission.csv', 'w') as f:
writer = csv.writer(f)
writer.writerows(self.resultingWay)
timeHour = int(self.timeSpentInMission / 60)
timeMin = self.timeSpentInMission - (timeHour * 60)
logger.info("The mission should take %d hour, %d min " %
(timeHour, timeMin))
def __createAndAddMissionItem(self, gpsX, gpsY, idType):
newItem = MissionItem(build_simple_mission_item(gpsX, gpsY, idType))
newItem.setID(idType)
self.__addMissionItem(newItem)
pass