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 state2odom(state):
'''
Converts a numpy state represenation to an Odometry message
@param ndarray(3,)/ndarray(5,): state
@return Odometry: odometry message
'''
odom = Odometry()
if state.shape[0] == 3:
odom.latitude_deg, odom.latitude_min = decimal2min(state[0])
odom.longitude_deg, odom.longitude_min = decimal2min(state[1])
odom.bearing_deg = state[2]
odom.speed = 0
return odom
elif state.shape[0] == 5:
odom.latitude_deg, odom.latitude_min = decimal2min(state[0])
odom.longitude_deg, odom.longitude_min = decimal2min(state[2])
odom.bearing_deg = state[4]
odom.speed = np.hypot(state[1], state[3])
return odom
async def run(self):
'''
Runs main loop for filtering data and publishing state estimates
'''
while True:
msg = None
sensor_data = self._getFreshData()
if self.filter is None:
# Need position and bearing to initialize
if sensor_data["pos"] is not None and sensor_data[
"bearing"] is not None:
msg = self._initFilter(sensor_data)
else:
if self.config["filter"] == "pipe":
# Need position and bearing to pipe
if sensor_data["pos"] is not None and sensor_data[
"bearing"] is not None:
msg = Odometry()
msg.latitude_deg, msg.latitude_min = decimal2min(
sensor_data["pos"][0])
msg.longitude_deg, msg.longitude_min = decimal2min(
sensor_data["pos"][1])
msg.bearing_deg = sensor_data["bearing"]
msg.speed = sensor_data["speed"] if sensor_data[
"speed"] is not None else 0.0
elif self.config["filter"] == "ekf":
# Need accel or pos to do anything
if sensor_data["accel"] is not None and sensor_data[
"rpy"] is not None:
ground_accel = sensor_data["accel"][0] * np.abs(
np.cos(sensor_data["rpy"][1]))
else:
ground_accel = None
msg = self.filter.iterate(ground_accel,
sensor_data["pos"],
sensor_data["bearing"],
rtk=self.gps.isRtk())
msg = state2odom(msg)
if msg is not None:
self.lcm.publish(self.config["odom_channel"], msg.encode())
# DEBUG
# print("[SensorFusion] Published odometry")
# printOdom(msg)
# print('\n')
else:
print("[SensorFusion] Unable to publish state.")
await asyncio.sleep(self.config["dt"])
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
def _initFilter(self, sensor_data):
'''
Constructs filter depending on filter type in config
@return Odometry: odometry LCM message to broadcast
'''
if self.config["filter"] == "ekf":
config_path = getenv('MROVER_CONFIG')
config_path += "/config_filter/ekf_config.json"
with open(config_path, "r") as config:
ekf_config = json.load(config)
if self.config["track_vel"]:
self.filter = EkfFusionVel(
ekf_config["vel"],
sensor_data["pos"],
0.0,
sensor_data["bearing"],
self.config["dt"],
ref_lat=self.config["ref_coords"]["lat"],
ref_lon=self.config["ref_coords"]["lon"],
rtk=self.gps.isRtk(),
filter_rtk=self.config["filter_rtk"])
else:
self.filter = EkfFusionNoVel(
ekf_config["no_vel"],
sensor_data["pos"],
sensor_data["bearing"],
self.config["dt"],
ref_lat=self.config["ref_coords"]["lat"],
ref_lon=self.config["ref_coords"]["lon"],
rtk=self.gps.isRtk(),
filter_rtk=self.config["filter_rtk"])
elif self.config["filter"] == "pipe":
self.filter = "pipe"
else:
raise ValueError("Invalid filter type!")
msg = Odometry()
msg.latitude_deg, msg.latitude_min = decimal2min(sensor_data["pos"][0])
msg.longitude_deg, msg.longitude_min = decimal2min(
sensor_data["pos"][1])
msg.bearing_deg = sensor_data["bearing"]
msg.speed = sensor_data["speed"] if sensor_data[
"speed"] is not None else 0.0
return msg
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())