def post_telemetry(self, location, heading):
"""
Post the drone's telemetry information.
:param location: The location to post.
:type location: Location
:param heading: The UAV's heading.
:type heading: Float
"""
missing_data = True
while missing_data:
try:
telem_upload_data = Telemetry(
location.get_lat(), location.get_lon(),
location.get_alt() + GCSSettings.MSL_ALT, heading)
missing_data = False
except ConnectionError:
sleep(GCSSettings.INTEROP_DISCONNECT_RETRY_RATE)
try:
self.client = Client(GCSSettings.INTEROP_URL,
GCSSettings.INTEROP_USERNAME,
GCSSettings.INTEROP_PASSWORD)
except:
print("Failed to connect to Interop., retrying...")
self.client.post_telemetry(telem_upload_data)
def rate_publisher(device, client):
pub = rospy.Publisher('/connections/judges/hz', Float32, queue_size=10)
rospy.init_node('interop', anonymous=True)
period = 4
mav = mavutil.mavlink_connection(device, autoreconnect=True)
sent_since_print = 0
last_print = rospy.get_time()
while not rospy.is_shutdown():
msg = mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True,
timeout=10.0)
if msg is None:
rospy.loginfo('Did not receive mavlink message')
sys.exit(1)
telemetry = Telemetry(latitude=mavlin_latlon(msg.lat),
longitude=mavlink_latlon(msg.lon),
altitude_msl=mavlink_alt(msg.alt),
uas_heading=mavlink_heading(msg.hdg))
try:
client.post_telemetry(telemetry)
except:
rospy.logerr('Telemetry Post Error')
sys.exit(1)
sent_since_print += 1
now = rospy.get_time()
since_print = now - last_print
if since_print > period:
telem_rate = sent_since_print / since_print
pub.publish(telem_rate)
sent_since_print = 0
last_print = now
def proxy_mavlink(device, client):
"""Receives packets over the device and forwards telemetry via the client.
Args:
device: A pymavlink device name to forward.
client: Interop Client with which to send telemetry packets.
"""
# Create the MAVLink connection.
mav = mavutil.mavlink_connection(device, autoreconnect=True)
# Track rates.
sent_since_print = 0
last_print = time.time()
client = interop.Client(url='http://10.42.0.1:8000',
username='******',
password='******')
# Continuously forward packets.
while True:
# Get packet.
msg = mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True,
timeout=10.0)
if msg is None:
logger.critical(
'Did not receive MAVLink packet for over 10 seconds.')
sys.exit(-1)
# Convert to telemetry.
telemetry = Telemetry(latitude=mavlink_latlon(msg.lat),
longitude=mavlink_latlon(msg.lon),
altitude_msl=mavlink_alt(msg.alt),
uas_heading=mavlink_heading(msg.hdg))
uas_head = mavlink_heading(msg.hdg)
lat = mavlink_latlon(msg.lat),
longi = mavlink_latlon(msg.lon),
alt = mavlink_alt(msg.alt)
#print uas_head
#print lat
#print longi
#print alt
client.post_telemetry(telemetry)
# Forward telemetry.
#try:
#client.post_telemetry(telemetry)
#except:
#logger.exception('Failed to post telemetry to interop.')
#sys.exit(-1)
# Track telemetry rates.
print(uas_head, lat, longi, alt)
sent_since_print += 1
now = time.time()
since_print = now - last_print
if since_print > PRINT_PERIOD:
logger.info('Telemetry rate: %f', sent_since_print / since_print)
sent_since_print = 0
last_print = now
def mavlink_packet(self, m):
'''handle a mavlink packet'''
if m.get_type() == 'GLOBAL_POSITION_INT':
telemetry = Telemetry(
float(m.lat) / 10000000,
float(m.lon) / 10000000,
float(m.alt) / 1000,
float(m.hdg) / 100)
self.client.post_telemetry(telemetry)
def telemetry(self, lat, lon, alt, heading):
t = Telemetry(latitude=lat,
longitude=lon,
altitude_msl=alt,
uas_heading=heading)
self.client.post_telemetry(t)
new_time = time()
print(1 / (new_time - self.last_telemetry))
self.last_telemetry = new_time
return True
def post_telemetry(self, location, heading):
"""
Post the drone's telemetry information
:param location: The location to post
:type location: Location
:param heading: The UAV's heading
:type heading: Float
"""
telem_upload_data = Telemetry(location.get_lat(), location.get_lon(), location.get_alt() + GCSSettings.MSL_ALT, heading)
self.client.post_telemetry(telem_upload_data)
def rate_publisher(client):
pub = rospy.Publisher('/connections/judges/hz', Float32, queue_size=10)
rospy.init_node('interop', anonymous=True)
period = 4
sent_since_print = 0
last_print = rospy.get_time()
while not rospy.is_shutdown():
telemetry = Telemetry(0, 0, 0, 0)
telemetry_resp = client.post_telemetry(telemetry)
sent_since_print += 1
now = rospy.get_time()
since_print = now - last_print
if since_print > period:
telem_rate = sent_since_print / since_print
pub.publish(telem_rate)
sent_since_print = 0
last_print = now
def probe(args, client):
while True:
start_time = datetime.datetime.now()
telemetry = Telemetry(0, 0, 0, 0)
telemetry_resp = client.post_telemetry(telemetry)
obstacle_resp = client.get_obstacles()
end_time = datetime.datetime.now()
elapsed_time = (end_time - start_time).total_seconds()
logger.info('Executed interop. Total latency: %f', elapsed_time)
delay_time = args.interop_time - elapsed_time
if delay_time > 0:
try:
time.sleep(delay_time)
except KeyboardInterrupt:
sys.exit(0)
def get_uas_telemetry(self, new_time):
delta_time = (new_time - self.time).total_seconds()
self.time = new_time
move_distance = self.speed * delta_time
delta_altitude = delta_time * max_climb_rate
delta_heading = delta_time * max_turn_rate
while move_distance > 0:
vec = self.pos.vector(self.path[self.waypoint])
# Way-point Switching
if vec.distance < switch_threshold:
if self.waypoint + 1 == len(self.path):
break
else:
self.waypoint += 1
continue
# Climb as needed
vec.altitude = max(vec.altitude, -delta_altitude)
vec.altitude = min(vec.altitude, delta_altitude)
delta_altitude -= abs(vec.altitude)
# Turn as needed
vec.vector.heading, delta_heading = calc_new_heading(
self.pos.heading, vec.heading, delta_heading)
vec.distance = move_distance if vec.distance > move_distance else vec.distance
move_distance -= vec.distance
# Apply motion
self.pos.move(vec)
return Telemetry(self.pos.latitude, self.pos.longitude,
self.pos.altitude, self.pos.heading)
def get_uas_telemetry(self, cur_time):
"""Overrides base method."""
return Telemetry(0, 0, 0, 0)
def proxy_mavlink(device, client):
"""Receives packets over the device and forwards telemetry via the client.
Args:
device: A pymavlink device name to forward.
client: Interop Client with which to send telemetry packets.
"""
# Create the MAVLink connection.
print(device)
mav = mavutil.mavlink_connection(device, baud=57600, autoreconnect=True)
# Track rates.
sent_since_print = 0
last_print = time.time()
# Continuously forward packets.
while True:
# Get packets
start = time.time()
alt_and_hdg = mav.recv_match(type= 'VFR_HUD',#'GLOBAL_POSITION_INT',
blocking=True,
timeout=10.0)
#msg = mav.recv_msg()
if alt_and_hdg is None:
logger.critical(
'Did not receive MAVLink packet for over 10 seconds.')
sys.exit(-1)
#received_VFR = time.time()
long_and_lat = mav.recv_match(type= 'GPS_RAW_INT',#'GLOBAL_POSITION_INT',
blocking=True,
timeout=10.0)
if long_and_lat is None:
logger.critical(
'Did not receive MAVLink packet for over 10 seconds.')
sys.exit(-1)
#received_GPS = time.time()
# Convert to telemetry.
telemetry = Telemetry(latitude=mavlink_latlon(long_and_lat.lat),
longitude=mavlink_latlon(long_and_lat.lon),
altitude_msl=mavlink_alt(alt_and_hdg.alt*1000),#mavlink_alt(msg.alt),
uas_heading=alt_and_hdg.heading)#mavlink_heading(alt_and_hdg.heading))
print(telemetry)
# Forward telemetry.
try:
client.post_telemetry(telemetry)
except:
logger.exception('Failed to post telemetry to interop.')
sys.exit(-1)
#posted_telemetry = time.time()
# Track telemetry rates.
sent_since_print += 1
now = time.time()
since_print = now - last_print
if since_print > PRINT_PERIOD:
logger.info('Telemetry rate: %f', sent_since_print / since_print)
sent_since_print = 0
last_print = now
def testTelemtry(client):
rand = generate_random_data(38.142544, -76.434088, 10000)
for w in rand:
telemetry = Telemetry(w['latitude'], w['longitude'], 200, 90)
client.post_telemetry(telemetry)
time.sleep(0.5)
def testWaypoints(client, mission):
for w in mission.mission_waypoints:
telemetry = Telemetry(w.latitude, w.longitude, w.altitude_msl, 90)
client.post_telemetry(telemetry)
# Tracks the uploads since last print.
sent_lock = threading.Lock()
last_print = datetime.datetime.now()
sent_since_print = 0
# Tracks upload success and prints any exceptions.
def handle_upload_result(future):
if future.exception():
print 'Request Failed. Exception: %s' % str(future.exception())
else:
with sent_lock:
sent_since_print += 1
# Continuously poll for new telemetry and send to server.
last_telemetry = Telemetry(0, 0, 0, 0)
while True:
telemetry = Telemetry(latitude=cs.lat,
longitude=cs.lng,
altitude=cs.alt,
uas_heading=cs.groundcourse)
if telemetry != last_telemetry:
client.post_telemetry(telemetry).add_done_callback(
handle_upload_result)
last_telemetry = telemetry
now = datetime.datetime.now()
since_print = (now - last_print).total_seconds()
if since_print >= PRINT_SEC:
with sent_lock:
local_sent_since_print = sent_since_print