def getStateByClosestTS(self, ts):
"""
Get the state 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 state measurement to this timestamp will be returned
@rtype: incoming_state
@return: An incoming_state object will all the recorded state information for the measurement
closest to the provided timestamp. Note that if the provided timestamp is lower than
all timestamp measurements or if the state table is empty, None will be returned.
"""
# test val:: 1541063324.1
# get gps as <= to the desired value. just easier todo that then get the absolute closest
selectGpsByTs = """SELECT id, date_part('epoch', time_stamp), roll, pitch, yaw
FROM incoming_state
WHERE incoming_state.time_stamp <= to_timestamp(%s) AT TIME ZONE 'UTC'
ORDER BY incoming_state.time_stamp DESC
LIMIT 1;"""
selectedState = super(IncomingStateDAO,
self).basicTopSelect(selectGpsByTs, (ts, ))
if selectedState is not None:
return incoming_state(selectedState)
return None
def test(self):
model = incoming_state()
model.time_stamp = 1547453775.2
model.roll = 40.111
model.pitch = -111.222
model.yaw = 12.3
truncateTable('incoming_state')
dao = IncomingStateDAO(defaultConfigPath())
self.assertIsNotNone(dao)
resultingId = dao.addState(model)
self.assertIsNotNone(resultingId)
self.assertNotEqual(resultingId, -1)
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_state')
dao = IncomingStateDAO(defaultConfigPath())
self.assertIsNotNone(dao)
baseTs = 1547453775.2
# test on empty table
resultModel = dao.getStateByClosestTS(baseTs)
self.assertIsNone(resultModel)
model = incoming_state()
model.time_stamp = baseTs
model.roll = 40.111
model.pitch = -111.222
model.yaw = 12.3
resultingId1 = dao.addState(model)
self.assertNotEqual(resultingId1, -1)
model.time_stamp = baseTs + 20000
model.roll = 40.222
model.pitch = -111.333
model.yaw = 567.8
resultingId2 = dao.addState(model)
self.assertNotEqual(resultingId2, -1)
# as explained above, this should return None
resultModel = dao.getStateByClosestTS(baseTs - 10000)
self.assertIsNone(resultModel)
resultModel = dao.getStateByClosestTS(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.getStateByClosestTS(baseTs + 15000)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.id, resultingId1)
# test when time is exactly equal
resultModel = dao.getStateByClosestTS(baseTs + 20000)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.id, resultingId2)
def setupIncomingStateTable():
model = incoming_state()
model.time_stamp = lowerTs
model.roll = 40.111
model.pitch = 111.222
model.yaw = 12.3
dao = IncomingStateDAO(defaultConfigPath())
assert dao.addState(model) != -1
model.time_stamp = upperTs
model.roll = 40.222
model.pitch = 111.333
model.yaw = 34.5
assert dao.addState(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_state()
model.time_stamp = 1547453775.2
model.roll = 40.111
model.pitch = -111.222
model.yaw = 12.3
truncateTable('incoming_state')
dao = IncomingStateDAO(defaultConfigPath())
self.assertIsNotNone(dao)
resultingId = dao.addState(model)
self.assertNotEqual(resultingId, -1)
gottenMeas = dao.getStateById(resultingId)
self.assertIsNotNone(gottenMeas)
self.assertEqual(gottenMeas.id, resultingId)
self.assertAlmostEqual(gottenMeas.time_stamp, 1547453775.2)
self.assertAlmostEqual(gottenMeas.roll, 40.111)
self.assertAlmostEqual(gottenMeas.pitch, -111.222)
self.assertAlmostEqual(gottenMeas.yaw, 12.3)
def getStateById(self, id):
"""
Get a state 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 state measurement to retrieve
@rtype: incoming_state
@returns: An incoming_state object with all recorded state 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
selectStateById = """SELECT id, date_part('epoch', time_stamp), roll, pitch, yaw
FROM incoming_State
WHERE id = %s LIMIT 1;"""
selectedState = super(IncomingStateDAO,
self).basicTopSelect(selectStateById, (id, ))
if selectedState is not None:
return incoming_state(selectedState)
return None