def get(self, id): dao = IncomingStateDAO(defaultConfigPath()) state = dao.getStateById(id) if state is None: return { 'message': 'Failed to locate state entry with id {}'.format(id) }, 404 return jsonify(state.toDict())
def get(self, ts): dao = IncomingStateDAO(defaultConfigPath()) state = dao.getStateByClosestTS(ts) if state is None: return { 'message': 'Failed to get closest state measurement by time_stamp. Either: a) the table is empty or b) the time_stampe is < all timestamps in the table' }, 404 return jsonify(state.toDict())
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 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): # 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 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 processGeolocation(self, geolocator, classification, croppedImg): """ Geolocation processing stuff common to both manual and autonomous. Called only by the processManualGeolocation and processAutonomousGeolocation methods """ dao = IncomingImageDAO(defaultConfigPath()) rawImg = dao.getImage(croppedImg.image_id) dao.close() if rawImg is None: print( "Failed to find raw image {} for autonomous cropped image {}!". format(croppedImg.image_id, croppedImg.crop_id)) return None dao = IncomingGpsDAO(defaultConfigPath()) gpsRaw = dao.getGpsByClosestTS(rawImg.time_stamp) dao.close() if gpsRaw is None: print("Failed to find gps measurement close to raw timestamp {}!". format(rawImg.time_stamp)) return None dao = IncomingStateDAO(defaultConfigPath()) stateRaw = dao.getStateByClosestTS(rawImg.time_stamp) dao.close() if stateRaw is None: print( "Failed to find state measurement close to raw timestamp {}!". format(rawImg.time_stamp)) return None # lat, lon = geolocator.calculate_geolocation(gpsRaw.lat, gpsRaw.lon, gpsRaw.alt, stateRaw.roll, stateRaw.pitch, stateRaw.yaw, croppedImg.crop_coordinate_tl.x, croppedImg.crop_coordinate_tl.y, croppedImg.crop_coordinate_br.x, croppedImg.crop_coordinate_br.y) lat, lon = geolocator.calculate_geolocation( gpsRaw.lat, gpsRaw.lon, -stateRaw.position[2], stateRaw.roll, stateRaw.pitch, stateRaw.yaw, croppedImg.crop_coordinate_tl.x, croppedImg.crop_coordinate_tl.y, croppedImg.crop_coordinate_br.x, croppedImg.crop_coordinate_br.y) return {'latitude': lat, 'longitude': lon}
class RosImagingHandler: """ This script is the bridge between ROS and the rest of the imaging system. It has two objectives: - listen to the ROS network and save relevant information to the server's database - take completed targets from the server's database and send them to interop over ROS Subscribes to the raw image, state, gps and focal_length ros topics """ STATE_SAVE_EVERY = 10 # save every 10th state messages (otherwise we get wayyy to many) 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 gpsCallback(self, msg): # changed to had 'stateMsg', see lines 52, 92 """ Ros subscriber callback. Subscribes to the inertial_sense GPS msg. Get the lla values from the GPS message and then pass them to the DAO so they can be inserted into the database """ if msg.fix_type == GPS.GPS_STATUS_FIX_TYPE_NO_FIX or msg.num_sat == 0: # dont insert if we dont have a gps fix yet return '''TODO Figure out how to replicate the above ^''' # TODO: Evaluate and Update self.gps_msg_.time_stamp = msg.header.stamp.to_sec() self.gps_msg_.lat = msg.latitude self.gps_msg_.lon = msg.longitude self.gps_msg_.alt = msg.altitude ''' Testing - Feb 2020: Goal: Pull GPS data from state, rather than GPS, as state is more accurate due to filtering Procedure: Because the state stores initial coordinates and distance from base in meters, we need to convert from meters to actual GPS points before storing. We do this using a geographiclib 'Direct' function, after estimating the angle between the two points. Notes: This procedure does result in rounding error. During a simulated test, we estimated this to be an average error of 0.01% (e.g., 0.1 m off for every 1000 m our plane is from the base station), although it did fluctuate (never surpassing 0.1 %). Overall, we believe the benefits will outweigh the error ''' ### New stuff starts here # north_dist = stateMsg.position[0] # east_dist = stateMsg.position[1] # estimated_azimuth = math.degrees(math.atan2(east_dist, north_dist)) # estimated_total_distance = math.sqrt( (north_dist**2) + (east_dist**2) ) # # Assumes we resolved the state initial lat/lon problem # estimated_mav_location = self.geod.Direct(stateMsg.initial_lat, stateMsg.initial_lon, estimated_azimuth, estimated_total_distance, Geodesic.STANDARD) # self.gps_msg_.time_stamp = stateMsg.header.stamp.to_sec() # self.gps_msg_.lat = estimated_mav_location['lat2'] # self.gps_msg_.lon = estimated_mav_location['lon2'] # self.gps_msg_.alt = stateMsg.initial_alt - stateMsg.position[2] ### ... and it ends here # insert into db: resultingId = self.gps_dao_.addGps(self.gps_msg_) if resultingId == -1: print("FAILED to insert gps measurement:") print("ts: {}, lat: {}, lon: {}, alt: {}".format( *self.gps_msg_.insertValues())) def stateCallback(self, msg): """ Ros subscriber callback. Subscribes to the /state rosplane topic. Passes the roll, pitch and yaw angle to be saved by the DAO. """ self.state_interval_ = (self.state_interval_ + 1) % self.STATE_SAVE_EVERY if self.state_interval_ != 0: return ts = msg.header.stamp.to_sec() self.state_msg_.time_stamp = ts # get rpy ANGLES self.state_msg_.roll = msg.phi self.state_msg_.pitch = msg.theta self.state_msg_.yaw = msg.psi resultingId = self.state_dao_.addState(self.state_msg_) if resultingId == -1: print("FAILED to insert state measurement:") print("ts: {}, roll: {}, pitch: {}, yaw: {}".format( *self.state_msg_.insertValues())) def rotate_about_center(self, src, angle, scale=1.): w = src.shape[1] h = src.shape[0] rangle = np.deg2rad(angle) # angle in radians # now calculate new image width and height nw = (abs(np.sin(rangle) * h) + abs(np.cos(rangle) * w)) * scale nh = (abs(np.cos(rangle) * h) + abs(np.sin(rangle) * w)) * scale # ask OpenCV for the rotation matrix rot_mat = cv2.getRotationMatrix2D((nw * 0.5, nh * 0.5), angle, scale) # calculate the move from the old center to the new center combined # with the rotation rot_move = np.dot(rot_mat, np.array([(nw - w) * 0.5, (nh - h) * 0.5, 0])) # the move only affects the translation, so update the translation # part of the transform rot_mat[0, 2] += rot_move[0] rot_mat[1, 2] += rot_move[1] return cv2.warpAffine(src, rot_mat, (int(np.ceil(nw)), int(np.ceil(nh)))) def imgCallback(self, msg): """ Ros subscriber callback. Subscribes to the cameras image topic. Saves the image file, and passes the corresponding filename and TS to the DAO so that it can be inserted into the database """ # create paths for where raw images will be dumped if necessary raw_path_ = os.path.join(getLatestBaseImgDir(), 'raw') if not os.path.exists(raw_path_): os.makedirs(raw_path_) # setup file name ts = msg.img.header.stamp.to_sec() filename = str(ts) + ".jpg" fullPath = os.path.join(raw_path_, filename) # setup the actual file data np_arr = np.fromstring(msg.img.data, np.uint8) image_np = cv2.imdecode( np_arr, cv2.IMREAD_COLOR ) # if its opencv < 3.0 then use cv2.CV_LOAD_IMAGE_COLOR self.img_msg_.time_stamp = ts self.img_msg_.image_path = fullPath self.img_msg_.focal_length = msg.focal_length orientation = msg.orientation # uint8 ORIENTATION_TYPE_ROTATE_180 = 3 # image is rotated 180 degrees # uint8 ORIENTATION_TYPE_ROTATE_90_CW = 6 # image is rotated 90 dec CW # uint8 ORIENTATION_TYPE_ROTATE_90_CCW = 8 # image is rotated 90 deg CCW if orientation == 3: # Rotated 180 properlyRotated = self.rotate_about_center(image_np, 180) elif orientation == 6: # rotated 90 CW properlyRotated = self.rotate_about_center(image_np, 90) elif orientation == 8: # rotated 90 CCW properlyRotated = self.rotate_about_center(image_np, 270) else: properlyRotated = image_np # write out the image cv2.imwrite(fullPath, properlyRotated) # insert into the db - returns db id of inserted image resultingId = self.img_dao_.addImage(self.img_msg_) if resultingId == -1: print("FAILED to insert image:") print( "ts: {}, path: {}, manual_tap: {}, autonomous_tap: {}".format( *self.img_msg_.insertValues())) def targetToInteropMsg(self, target): """ Convert a submitted_target model to an InteropImage msg returns an InteropImage msg ready for publishing """ img = None # figure out the image part of the message if hasattr(target, 'crop_path'): cv_img = cv2.imread(target.crop_path) try: img = self.bridge.cv2_to_imgmsg(cv_img, "bgr8") except CvBridgeError as e: print(e) # this dictionary will hold all the values that we care about # pushing into the message for whatever target type we're dealing with # ie: for emergent it sets irrelevant values to none, to ensure they arent # posted to the judges targetDict = target.toAuvsiJson() targetDict["image"] = img return targetDict def submitPendingTargets(self): # if there are people actually subscribed to this topic if '/imaging/target' not in rosservice.get_service_list(): # print(Submission service not yet published") return # if the service exists on the network if self.submit_image_ is None: # if this is our first time connecting to the image submit service self.submit_image_ = rospy.ServiceProxy('/imaging/target', SubmitImage) target_dao = SubmittedTargetDAO(self.configPath) # if there are targets waiting to be submitted to the judges if not target_dao.areTargetsPending(): print("no targets pending") return # then lets submit them pending = target_dao.getAllPendingTargets() if pending is None or not pending: return for target in pending: imageMsg = self.targetToInteropMsg(target) resp = self.submit_image_( ** imageMsg) # map dictionary into function args for submit_image if resp.success: # only set a target as submitted if interop says it was successful target_dao.setTargetSubmitted(target.target, target.autonomous) def run(self): # as per (this very old thread): http://ros-users.122217.n3.nabble.com/ros-spinOnce-equivalent-for-rospy-td2317347.html # publishers/subscribers in rospy are automatically run in serperate threads, # meaning we dont need to worry about spinning to respond to subscriber callbacks while not rospy.is_shutdown(): self.submitPendingTargets() # see if there's anything to publish rospy.sleep(1) # sleep for one second
def test(self): dao = IncomingStateDAO(defaultConfigPath()) self.assertIsNotNone(dao) self.assertIsNotNone(dao.conn)