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)