def test(self):
model = incoming_image()
model.time_stamp = 1547453775.2
model.focal_length = 16.0
model.image_path = '/im/a/totally/real/path/i/swear.jpg'
model.manual_tap = False
model.autonomous_tap = False
truncateTable('incoming_image')
dao = IncomingImageDAO(defaultConfigPath())
self.assertIsNotNone(dao)
# test with empty table
self.assertIsNone(dao.getImage(1))
resultingId = dao.addImage(model)
self.assertNotEqual(resultingId, -1)
resultingModel = dao.getImage(resultingId)
self.assertIsNotNone(resultingModel)
self.assertAlmostEqual(resultingModel.time_stamp, model.time_stamp)
self.assertEqual(resultingModel.focal_length, model.focal_length)
self.assertEqual(resultingModel.image_path, model.image_path)
self.assertEqual(resultingModel.manual_tap, model.manual_tap)
self.assertEqual(resultingModel.autonomous_tap, model.autonomous_tap)
def get(self, image_id):
dao = IncomingImageDAO(defaultConfigPath())
image = dao.getImage(image_id)
if image is None:
return {
'message': 'Failed to locate raw id {}'.format(image_id)
}, 404
return jsonify(image.toDict())
def get(self, image_id):
dao = IncomingImageDAO(defaultConfigPath())
image = dao.getImage(image_id)
if image is None:
return {
'message': 'Failed to locate raw id {}'.format(image_id)
}, 404
# otherwise lets send the image::
return rawImageSender(image.image_id, image.image_path)
def get(self):
# startTime = time.time()
# Input Validation::
manual = checkXManual(request)
# Get Content
dao = IncomingImageDAO(defaultConfigPath())
image = dao.getNextImage(manual)
# response validation
if image is None:
return {'message': 'Failed to locate untapped image'}, 404
# print("Request fulfillment: {}".format(time.time()-startTime))
return rawImageSender(image.image_id, image.image_path)
def setupIncomingImageTable(rawImgPath):
dao = IncomingImageDAO(defaultConfigPath())
incModel = incoming_image()
incModel.focal_length = 16.0
incModel.time_stamp = 1547453775.2
incModel.image_path = rawImgPath
incModel.manual_tap = False
incModel.autonomous_tap = False
incImageId1 = dao.addImage(incModel)
assert incImageId1 != -1
# just gonna use a slightly different number so we can tell the difference between the two inserts
incModel.focal_length = 17.0
incImageId2 = dao.addImage(incModel)
assert incImageId2 != -1
return (incImageId1, incImageId2)
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):
truncateTable('incoming_image')
truncateTable('cropped_autonomous')
truncateTable('outgoing_autonomous')
dao = OutgoingAutonomousDAO(defaultConfigPath())
testIns = outgoing_autonomous()
testIns.crop_id = 42
testIns.shape = 'circle'
testIns.background_color = 'white'
testIns.alphanumeric = 'A'
testIns.alphanumeric_color = 'black'
self.assertNotEqual(dao.addClassification(testIns), -1)
dao = CroppedAutonomousDAO(defaultConfigPath())
model = cropped_autonomous()
model.image_id = 123
model.time_stamp = 1547453775.2
model.cropped_path = '/im/a/totally/real/cropped/path/i/swear.jpg'
model.crop_coordinate_br = "(12, 34)"
model.crop_coordinate_tl = "(56, 78)"
self.assertNotEqual(dao.addImage(model), -1)
dao = IncomingImageDAO(defaultConfigPath())
model = incoming_image()
model.time_stamp = 1547453775.2
model.focal_length = 16.0
model.image_path = '/im/a/totally/real/path/i/swear.jpg'
model.manual_tap = True
model.autonomous_tap = True
resultingId = dao.addImage(model)
self.assertNotEqual(resultingId, -1)
util = UtilDAO(defaultConfigPath())
util.resetAutonomousDB()
resultingModel = dao.getImage(resultingId)
self.assertIsNotNone(resultingModel)
self.assertFalse(resultingModel.autonomous_tap)
self.assertTrue(resultingModel.manual_tap)
self.assertEqual(resultingModel.image_path, model.image_path)
self.assertEqual(resultingModel.focal_length, model.focal_length)
dao = CroppedAutonomousDAO(defaultConfigPath())
self.assertEqual(len(dao.getAll()), 0)
dao = OutgoingAutonomousDAO(defaultConfigPath())
self.assertEqual(len(dao.getAll()), 0)
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 = IncomingImageDAO(defaultConfigPath())
self.assertIsNotNone(dao)
self.assertIsNotNone(dao.conn)
def test(self):
truncateTable('incoming_image')
dao = IncomingImageDAO(defaultConfigPath())
self.assertIsNotNone(dao)
# test with empty table
self.assertIsNone(dao.getNextImage(True))
self.assertIsNone(dao.getNextImage(False))
model = incoming_image()
model.time_stamp = 1547453775.2
model.focal_length = 16.0
model.image_path = '/im/a/totally/real/path/i/swear.jpg'
model.manual_tap = False
model.autonomous_tap = False
resultingId = dao.addImage(model)
self.assertNotEqual(resultingId, -1)
# identical timestamps should make no difference
model.focal_length = 16.0
model.image_path = '/im/a/totally/real/path/2/i/swear.jpg'
resultingId2 = dao.addImage(model)
self.assertNotEqual(resultingId2, -1)
resultModel = dao.getNextImage(True)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.image_id, resultingId)
self.assertTrue(resultModel.manual_tap)
self.assertFalse(resultModel.autonomous_tap)
resultModel = dao.getNextImage(True)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.image_id, resultingId2)
self.assertTrue(resultModel.manual_tap)
self.assertFalse(resultModel.autonomous_tap)
resultModel = dao.getNextImage(False)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.image_id, resultingId)
self.assertTrue(resultModel.manual_tap)
self.assertTrue(resultModel.autonomous_tap)
resultModel = dao.getNextImage(True)
self.assertIsNone(resultModel)
resultModel = dao.getNextImage(False)
self.assertIsNotNone(resultModel)
self.assertEqual(resultModel.image_id, resultingId2)
self.assertTrue(resultModel.manual_tap)
self.assertTrue(resultModel.autonomous_tap)
resultModel = dao.getNextImage(False)
self.assertIsNone(resultModel)
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}
def post(self):
# confirm that the X-Manual header was specified
manual = checkXManual(request)
# basic setup pieces for this taken from :
# http://flask.pocoo.org/docs/1.0/patterns/fileuploads/
# Input Validation:
if 'cropped_image' not in request.files:
abort(400, "Need to pass an image with the key 'cropped_image'")
imageFile = request.files.get('cropped_image', '')
# use the right model object depending on if this is a manual or autonomous request
cropped = cropped_manual() if manual else cropped_autonomous()
# confirm that we got an image_id in the headers
if 'X-Image-Id' in request.headers:
cropped.image_id = request.headers.get('X-Image-Id')
else:
abort(400, "Need to specify header 'X-Image-Id'!")
# make sure the filename wont make our computer explode:
if imageFile.filename == '' or imageFile.filename == 'cropped_image':
# if we didnt get a filename we assume jpg. why not....
extension = 'jpg'
elif not allowedFileType(imageFile.filename):
abort(400, "Filetype is not allowed!")
else:
extension = getFileExtension(imageFile.filename)
imageFile.filename = str(int(time.time())) + "-" + str(
randint(0, 10000)) + '.' + extension
filename = secure_filename(imageFile.filename)
# save image
full_path = os.path.join(defaultCroppedImgPath(), filename)
imageFile.save(full_path)
cropped.cropped_path = full_path
# check for the optional crop coordinate form data
if 'crop_coordinate_tl' in request.form:
cropped.crop_coordinate_tl = point(
ptStr=request.form['crop_coordinate_tl'])
if 'crop_coordinate_br' in request.form:
cropped.crop_coordinate_br = point(
ptStr=request.form['crop_coordinate_br'])
# get the timestamp from incoming_image table to copy into this guy....
# meh, but functional /shrug
dao = IncomingImageDAO(defaultConfigPath())
img = dao.getImage(cropped.image_id)
if img is not None:
cropped.time_stamp = img.time_stamp
else:
print(
"WARN:: failed to find incoming_image model at id {} X-Image-Id"
.format(cropped.image_id))
cropped.time_stamp = int(time.time())
# add to db
dao = CroppedManualDAO(
defaultConfigPath()) if manual else CroppedAutonomousDAO(
defaultConfigPath())
# resultingId is the cropped_manual.id value given to this image (abstracted from client)
resultingId = dao.upsertCropped(cropped)
if resultingId == -1:
return {
'message':
'Failed to insert image into manual_cropped! (If youre trying to update information on an image_id that already exists, you should use PUT)'
}, 500
# done!
response = make_response(
jsonify({
'message': 'success!',
'id': resultingId
}))
response.headers['X-Crop-Id'] = resultingId
return response