def run(self):
# wait until we get our first gps coordinate
# remember - the ros_handler code does not insert a gps record
# into the table until the gps has a fix
haveGps = False
print("waiting for gps")
while not haveGps and not self._should_shutdown:
# check for any entries by just getting all
dao = IncomingGpsDAO(defaultConfigPath())
results = dao.getAll()
dao.close()
if results is None:
time.sleep(1)
else:
haveGps = True # we've got something!
print(
"Have gps! Using the first coordinate as our ground station. Starting geolocation..."
)
# get the coordinates from the first recorded gps measurement
# this will be the coordinates we use for the groundstation
# in geolocation
dao = IncomingGpsDAO(defaultConfigPath())
groundstationGps = dao.getFirst()
dao.close()
# now we can run stuff
geo = targetGeolocation(groundstationGps.lat, groundstationGps.lon)
while not self._should_shutdown:
# lets deal with manual classifications in the queue
dao = OutgoingManualDAO(defaultConfigPath())
classifications = dao.getUnlocatedClassifications()
dao.close()
if classifications is not None:
for classification in classifications:
self.processManualGeolocation(geo, classification)
# now lets do autonomous
dao = OutgoingAutonomousDAO(defaultConfigPath())
classifications = dao.getUnlocatedClassifications()
dao.close()
if classifications is not None:
for classification in classifications:
self.processAutonomousGeolocation(geo, classification)
time.sleep(1)
def generalTest(self):
testGndLat = 40.248471
testGndLon = -111.645821
testMavLat = 40.246531
testMavLon = -111.648298
testHeight = 16
testRoll = 0
testPitch = 60
testYaw = 90
testTLCoorX = 1986
testTLCoorY = 969
testBRCoorX = 1988
testBRCoorY = 971
system = targetGeolocation(testGndLat, testGndLon)
targetLocation = system.calculate_geolocation(testMavLat, testMavLon, testHeight, testRoll, testPitch, testYaw, testTLCoorX, testTLCoorY, testBRCoorX, testBRCoorY)
self.assertAlmostEqual(testGndLat, targetLocation[0], places=3)
self.assertAlmostEqual(testGndLon, targetLocation[1], places=3)
def testZeroStateData(self):
testGndLat = 40.248471
testGndLon = -111.645821
testMavLat = 40.174375
testMavLon = -111.655704
testHeight = 16
testRoll = 0
testPitch = 0
testYaw = 0
testTLCoorX = 999
testTLCoorY = 999
testBRCoorX = 1001
testBRCoorY = 1001
system = targetGeolocation(testGndLat, testGndLon)
targetLocation = system.calculate_geolocation(testMavLat, testMavLon, testHeight, testRoll, testPitch, testYaw, testTLCoorX, testTLCoorY, testBRCoorX, testBRCoorY)
self.assertAlmostEqual(testMavLat, targetLocation[0], places=3)
self.assertAlmostEqual(testMavLon, targetLocation[1], places=3)
def testArbitraryData(self):
testLat = 1000
testLon = 2000
testMavLat = 250
testMavLon = 120
testHeight = 20
testRoll = 20
testPitch = 45
testYaw = 95
testTLCoorX = 200
testTLCoorY = 200
testBRCoorX = 400
testBRCoorY = 400
system = targetGeolocation(testLat, testLon)
system.calculate_geolocation(testMavLat, testMavLon, testHeight, testRoll, testPitch, testYaw, testTLCoorX, testTLCoorY, testBRCoorX, testBRCoorY)
self.assertEqual(system.lat_mav, testMavLat)
self.assertEqual(system.lon_mav, testMavLon)
self.assertEqual(system.phi_deg, testRoll)
self.assertEqual(system.theta_deg, testPitch)
self.assertEqual(system.TopLeftX, testTLCoorX)
self.assertEqual(system.TopLeftY, testTLCoorY)
self.assertEqual(system.BottomRightX, testBRCoorX)
self.assertEqual(system.BottomRightY, testBRCoorY)
def testConstructor(self):
testLat = 1000
testLon = 2000
system = targetGeolocation(testLat, testLon)
self.assertEqual(system.lat_gnd, testLat)
self.assertEqual(system.lon_gnd, testLon)