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