def getGpsByClosestTS(self, ts):
"""
Get the gps row that has a time_stamp closest to the one specified.
This will likely be the method most used by geolocation and autonomous
localization methods.
@type ts: float
@param ts: UTC Unix Epoch timestamp as a float. The closest gps measurement to this timestamp will be returned
@rtype: incoming_gps
@return: An incoming_gps object will all the recorded gps information for the measurement
closest to the provided timestamp. Note that if the provided timestamp is lower than
all timestamp measurements or if the gps table is empty, None will be returned.
"""
# get gps as <= to the desired value. just easier todo that then get the absolute closest
selectGpsByTs = """SELECT id, date_part('epoch', time_stamp), latitude, longitude, altitude
FROM incoming_gps
WHERE incoming_gps.time_stamp <= to_timestamp(%s) AT TIME ZONE 'UTC'
ORDER BY incoming_gps.time_stamp DESC
LIMIT 1;"""
selectedGps = super(IncomingGpsDAO, self).basicTopSelect(selectGpsByTs, (ts,))
if selectedGps is not None:
return incoming_gps(selectedGps)
return None
def test(self):
model = incoming_gps()
model.time_stamp = 1547453775.2
model.lat = 40.111
model.lon = -111.222
model.alt = 1234.5
truncateTable('incoming_gps')
dao = IncomingGpsDAO(defaultConfigPath())
resultingId = dao.addGps(model)
self.assertIsNotNone(resultingId)
self.assertNotEqual(resultingId, -1)
def getFirst(self):
"""
Get the first (aka oldest) GPS measurement in the table
"""
selectOldest = """SELECT id, date_part('epoch', time_stamp), latitude, longitude, altitude
FROM incoming_gps
ORDER BY id ASC LIMIT 1;"""
selectedGps = super(IncomingGpsDAO, self).basicTopSelect(selectOldest, None)
if selectedGps is not None:
return incoming_gps(selectedGps)
return None
def test(self):
# Note: the closest TS function doesnt work by absolute closest,
# but instead for ease, speed and readability, just checks <=
# These tests reflect this type of functionality and would probably have
# to be redone if true closest TS was ever implemented
truncateTable('incoming_gps')
dao = IncomingGpsDAO(defaultConfigPath())
self.assertIsNotNone(dao)
baseTs = 1547453775.2
# test on empty table
resultModel = dao.getGpsByClosestTS(baseTs)
self.assertIsNone(resultModel)
model = incoming_gps()
model.time_stamp = baseTs
model.lat = 40.111
model.lon = -111.222
model.alt = 1234.5
resultingId1 = dao.addGps(model)
self.assertNotEqual(resultingId1, -1)
model.time_stamp = baseTs + 20000
model.lat = 40.222
model.lon = -111.333
model.alt = 5678.9
resultingId2 = dao.addGps(model)
self.assertNotEqual(resultingId2, -1)
# as explained above, this should return None
resultModel = dao.getGpsByClosestTS(baseTs - 10000)
self.assertIsNone(resultModel)
resultModel = dao.getGpsByClosestTS(baseTs + 1000)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.id, resultingId1)
# while this is absolutely closer to id2, we should
# still get id1 for the spec reasons described at the
# top of this method
resultModel = dao.getGpsByClosestTS(baseTs + 15000)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.id, resultingId1)
# test when time is exactly equal
resultModel = dao.getGpsByClosestTS(baseTs + 20000)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.id, resultingId2)
def test(self):
truncateTable("incoming_gps")
dao = IncomingGpsDAO(defaultConfigPath())
# empty table
self.assertIsNone(dao.getAll())
# insert a couple rows
model = incoming_gps()
baseTs = 1547453775.2
model.time_stamp = baseTs
model.lat = 40.111
model.lon = -111.222
model.alt = 1234.5
resultingId1 = dao.addGps(model)
self.assertNotEqual(resultingId1, -1)
model.time_stamp = baseTs + 20000
model.lat = 40.222
model.lon = -111.333
model.alt = 5678.9
resultingId2 = dao.addGps(model)
self.assertNotEqual(resultingId2, -1)
results = dao.getAll()
self.assertIsNotNone(results)
self.assertEqual(len(results), 2)
if results[0].id == resultingId1:
self.assertAlmostEqual(results[0].lat, 40.111)
self.assertAlmostEqual(results[0].lon, -111.222)
self.assertAlmostEqual(results[0].alt, 1234.5)
self.assertAlmostEqual(results[1].lat, 40.222)
self.assertAlmostEqual(results[1].lon, -111.333)
self.assertAlmostEqual(results[1].alt, 5678.9)
elif results[0].id == resultingId2:
self.assertAlmostEqual(results[1].lat, 40.111)
self.assertAlmostEqual(results[1].lon, -111.222)
self.assertAlmostEqual(results[1].alt, 1234.5)
self.assertAlmostEqual(results[0].lat, 40.222)
self.assertAlmostEqual(results[0].lon, -111.333)
self.assertAlmostEqual(results[0].alt, 5678.9)
else:
self.fail("dont recognize one of the ids returned by gps.getAll")
def setupIncomingGpsTable():
model = incoming_gps()
model.time_stamp = lowerTs
model.lat = 40.111
model.lon = -111.222
model.alt = 1234.5
dao = IncomingGpsDAO(defaultConfigPath())
assert dao.addGps(model) != -1
model.time_stamp = upperTs
model.lat = 40.222
model.lon = -111.333
model.alt = 567.8
assert dao.addGps(model) != -1
def __init__(self):
print("Startup NEW (Mar 2020) ros imaging handler...")
currentPath = os.path.dirname(os.path.realpath(__file__))
self.configPath = rospy.get_param('~config_path', defaultConfigPath())
self.bridge = CvBridge()
# gps ingestion setup:
self.gps_dao_ = IncomingGpsDAO(self.configPath)
self.gps_subscriber_ = rospy.Subscriber('/gps',
GPS,
self.gpsCallback,
queue_size=10)
# self.gps_subscriber_ = rospy.Subscriber('/state', State, self.gpsCallback, queue_size=10)
self.gps_msg_ = incoming_gps()
# imaging ingestion setup:
self.img_dao_ = IncomingImageDAO(self.configPath)
self.img_subscriber_ = rospy.Subscriber(
"/a6000_ros_node/img/compressed",
CompressedImgWithMeta,
self.imgCallback,
queue_size=10)
self.img_msg_ = incoming_image()
self.img_msg_.focal_length = 16.0 # this is a safe starting assumption == fully zoomed out on a6000
self.img_msg_.manual_tap = False
self.img_msg_.autonomous_tap = False
# state ingestion setup:
self.state_dao_ = IncomingStateDAO(self.configPath)
self.state_subscriber_ = rospy.Subscriber('/state',
State,
self.stateCallback,
queue_size=10)
self.state_msg_ = incoming_state()
self.state_interval_ = 0
basePath = createNewBaseImgDir()
print("Base dir for images:: {}".format(basePath))
# service for completed targets. Once a target has gone through the entire
# system, the server puts the finished target in a table and marks it ready for submission
self.submit_image_ = None
print("ROS subscribers are all setup!")
self.geod = Geodesic.WGS84
def test(self):
model = incoming_gps()
model.time_stamp = 1547453775.2
model.lat = 40.111
model.lon = -111.222
model.alt = 1234.5
truncateTable('incoming_gps')
dao = IncomingGpsDAO(defaultConfigPath())
self.assertIsNotNone(dao)
resultingId = dao.addGps(model)
self.assertNotEqual(resultingId, -1)
gottenMeas = dao.getGpsById(resultingId)
self.assertIsNotNone(gottenMeas)
self.assertEqual(gottenMeas.id, resultingId)
self.assertAlmostEqual(gottenMeas.time_stamp, 1547453775.2)
self.assertAlmostEqual(gottenMeas.lat, 40.111)
self.assertAlmostEqual(gottenMeas.lon, -111.222)
self.assertAlmostEqual(gottenMeas.alt, 1234.5)
def getGpsById(self, id):
"""
Get a gps measurement from the database by its id. This will likely only be
used internally, if at all.
@type id: int
@param id: The unique id of the gps measurement to retrieve
@rtype: incoming_gps
@returns: An incoming_gps object with all recorded gps information if the measurement
with the given id exists, otherwise None.
"""
# note we need the date_part to convert the time_stamp back to unix epoch time
selectGpsById = """SELECT id, date_part('epoch', time_stamp), latitude, longitude, altitude
FROM incoming_gps
WHERE id = %s LIMIT 1;"""
selectedGps = super(IncomingGpsDAO, self).basicTopSelect(selectGpsById, (id,))
if selectedGps is not None:
return incoming_gps(selectedGps)
return None
def newModelFromRow(self, row, json=None):
"""
Create a new incoming_gps model from the given info
"""
return incoming_gps(row)