def main():
while True:
try:
time.sleep(1)
with get_android() as android:
print('connected to Samsung GS3')
while True:
raw_data = android.read()
if raw_data:
frame = AndroidFrame(raw_data)
odom_msg = Odometry()
odom_msg.latitude_deg = int(frame.lat_deg)
odom_msg.latitude_min = frame.lat_min
odom_msg.longitude_deg = int(frame.lon_deg)
odom_msg.longitude_min = frame.lon_min
odom_msg.bearing_deg = frame.azimuth_deg
odom_msg.speed = 0
lcm_.publish('/odometry', odom_msg.encode())
bearing_msg = Bearing()
bearing_msg.bearing = frame.azimuth_deg
lcm_.publish('/bearing', bearing_msg.encode())
else:
print('read timed out, retrying the connection')
break
except usb.core.USBError:
traceback.print_exception(*sys.exc_info())
print('USBError occured, retrying...')
def asOdom(self):
'''
Returns the current state estimate as an Odometry LCM object
@return Odometry: state estimate in Odometry LCM format
'''
odom = Odometry()
odom.latitude_deg = self.pos["lat_deg"]
odom.latitude_min = self.pos["lat_min"]
odom.longitude_deg = self.pos["long_deg"]
odom.longitude_min = self.pos["long_min"]
odom.bearing_deg = self.bearing_deg
odom.speed = np.hypot(self.vel["north"], self.vel["east"])
return odom
async def transmit_fake_odometry():
while True:
new_odom = Odometry()
new_odom.latitude_deg = random.randint(42, 43)
new_odom.longitude_deg = random.randint(-84, -82)
new_odom.latitude_min = random.uniform(0, 60)
new_odom.longitude_min = random.uniform(0, 60)
new_odom.bearing_deg = random.uniform(0, 359)
with await lock:
lcm_.publish('/odom', new_odom.encode())
print("Published new odometry")
await asyncio.sleep(0.5)
def main():
lcm = aiolcm.AsyncLCM()
serialPort = serial.Serial('/dev/ttyUSB0')
serialPort.baudrate = 4800
serialPort.bytesize = serial.EIGHTBITS
serialPort.parity = serial.PARITY_NONE
serialPort.stopbits = serial.STOPBITS_ONE
while (True):
oneByte = serialPort.read()
if oneByte != b'$':
continue
fiveBytes = serialPort.read(5)
if fiveBytes != b'GPRMC':
continue
odometry = Odometry()
serialPort.read_until(b',')[:-1]
serialPort.read_until(b',')[:-1]
hasFix = serialPort.read_until(b',')[:-1]
if hasFix != b'A':
continue
latitude = float(serialPort.read_until(b',')[:-1])
serialPort.read_until(b',')[:-1]
longitude = float(serialPort.read_until(b',')[:-1])
serialPort.read_until(b',')[:-1]
speed = float(serialPort.read_until(b',')[:-1])
bearing = float(serialPort.read_until(b',')[:-1])
odometry.latitude_deg = int(latitude / 100)
odometry.longitude_deg = int(longitude / 100)
odometry.latitude_min = latitude - (odometry.latitude_deg * 100)
odometry.longitude_min = longitude - (odometry.longitude_deg * 100)
odometry.bearing_deg = bearing
odometry.speed = speed
lcm.publish('/odometry', odometry.encode())