def main(nh, ci, course, freq):
freq=27440
boat = yield boat_scripting.get_boat(nh)
#float_df = boat.float()
#yield boat.retract_hydrophone() # why was this here? it shouldn't need this, the hydrophones should be up on start
print "Starting ping mission"
print "Deploying Hydrophone"
yield boat.deploy_hydrophone()
print "Hydrophone Deployed"
try:
yield util.wrap_timeout(boat.hydrophone_align(freq), 60*2)
except Exception:
traceback.print_exc()
print "Finished ping mission"
# Note: /absodom is Earth-Centered,Earth-Fixed (ECEF), so This means that ECEF rotates with the earth and a point fixed on the surface of the earth do not change.
# See: http://en.wikipedia.org/wiki/ECEF
msg = yield boat.get_gps_odom()
temp = gps.latlongheight_from_ecef([msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z])
print "latitude: ", temp[0]," longitude: ", temp[1]
_send_result(ci, course, math.degrees(temp[0]), math.degrees(temp[1])) # async XXX
#float_df.cancel()
print "Retracting Hydrophone"
yield boat.retract_hydrophone()
print "Hydrophone Retracted"
print "Done"
def enu(self, point):
'''
Produces enu, ecef, and lla from enu point.
'''
enu = point
enu_vector = enu - self.enu_pos
ecef_vector = enu_from_ecef_tf(self.ecef_pos).T.dot(enu_vector)
ecef = ecef_vector + self.ecef_pos
lla = np.degrees(latlongheight_from_ecef(ecef))
return enu, ecef, lla
def ecef(self, point):
'''
Produces enu, ecef, and lla from ecef point.
'''
ecef = point
lla = np.degrees(latlongheight_from_ecef(ecef))
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def ecef(self, point):
'''
Produces enu, ecef, and lla from ecef point.
'''
ecef = point
lla = np.degrees(latlongheight_from_ecef(ecef))
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def enu(self, point):
'''
Produces enu, ecef, and lla from enu point.
'''
enu = point
enu_vector = enu - self.enu_pos
ecef_vector = enu_from_ecef_tf(self.ecef_pos).T.dot(enu_vector)
ecef = ecef_vector + self.ecef_pos
lla = np.degrees(latlongheight_from_ecef(ecef))
return enu, ecef, lla
def enu(self):
enu = self.point
# to ecef
enu_vector = enu - self.enu_pos
ecef_vector = enu_from_ecef_tf(self.ecef_pos).T.dot(enu_vector)
ecef = ecef_vector + self.ecef_pos
# to lla
lla = np.degrees(latlongheight_from_ecef(ecef))
return enu, ecef, lla
def ecef(self):
ecef = self.point
# to lla
lla = np.degrees(latlongheight_from_ecef(ecef))
# to enu
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def enu(self):
enu = self.point
# to ecef
enu_vector = enu - self.enu_pos
ecef_vector = enu_from_ecef_tf(self.ecef_pos).T.dot(enu_vector)
ecef = ecef_vector + self.ecef_pos
# to lla
lla = np.degrees(latlongheight_from_ecef(ecef))
return enu, ecef, lla
def ecef(self):
ecef = self.point
# to lla
lla = np.degrees(latlongheight_from_ecef(ecef))
# to enu
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
