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 odometry_cb(self, channel, msg):
m = Odometry.decode(msg)
self.OdometryMsg.latitude_deg = m.latitude_deg
self.OdometryMsg.latitude_min = m.latitude_min
self.OdometryMsg.longitude_deg = m.longitude_deg
self.OdometryMsg.longitude_min = m.longitude_min
self.OdometryMsg.bearing_deg = m.bearing_deg
self.OdometryMsg.speed = m.speed
def odom_callback(self, channel, msg):
odom = Odometry.decode(msg)
if (time.time()*1000 - self.odom_millis) > \
self.logConfig['rate_millis']['odom']:
self.write([
odom.latitude_deg, odom.latitude_min, odom.longitude_deg,
odom.longitude_min, odom.bearing_deg, odom.speed
], 'odom')
self.odom_millis = time.time() * 1000
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
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)
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 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())
def odomCallback(self, channel, msg):
new_odom_msg = Odometry.decode(msg)
lat = min2decimal(new_odom_msg.latitude_deg, new_odom_msg.latitude_min)
lon = min2decimal(new_odom_msg.longitude_deg,
new_odom_msg.longitude_min)
new_odom = np.array([[
0.0,
lat2meters(lat, ref_lat=self.ref_coords["lat"]),
lon2meters(lon, lat, ref_lon=self.ref_coords["lon"]),
new_odom_msg.bearing_deg, new_odom_msg.speed
]])
if self.odom_history is None:
self.odom_history = new_odom
else:
new_odom[0, 0] = self.odom_history[-1, 0] + self.sim_config["dt"]
self.odom_history = np.vstack([self.odom_history, new_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 __init__(self): # other args if ya want em
# all initially defined variables should be here
# while not technically globals, this is basically where they are
# defined for the sim, since the entire instance is the SimMetaClass
# below: class list, one class for each message type
# published or recieved. instantiate them at the bottom
# of this message definition block
# use the provided imported classes and dump these later
# you still need to set all the defaults
self.AutonStateMsg = AutonState()
self.AutonStateMsg.is_auton = False
self.CourseMsg = Course()
self.CourseMsg.num_waypoints = 0
self.CourseMsg.hash = 0
self.CourseMsg.waypoints = []
self.GPSMsg = GPS()
self.GPSMsg.latitude_deg = 39
self.GPSMsg.latitude_min = 0
self.GPSMsg.longitude_deg = -110
self.GPSMsg.longitude_min = 0
self.GPSMsg.bearing_deg = 0
self.GPSMsg.speed = -999 # this value is never used
# so it's being set to a dummy value. DO NOT USE IT
self.JoystickMsg = Joystick()
self.JoystickMsg.forward_back = 0
self.JoystickMsg.left_right = 0
self.JoystickMsg.dampen = 0
self.JoystickMsg.kill = False
self.JoystickMsg.restart = False
self.NavStatusMsg = NavStatus()
self.NavStatusMsg.nav_state = 0
self.NavStatusMsg.nav_state_name = ""
self.NavStatusMsg.completed_wps = 0
self.NavStatusMsg.missed_wps = 0
self.NavStatusMsg.total_wps = 0
self.NavStatusMsg.found_tbs = 0
self.NavStatusMsg.total_tbs = 0
self.ObstacleMsg = Obstacle()
self.ObstacleMsg.detected = False
self.ObstacleMsg.bearing = 0.0
self.ObstacleMsg.distance = 0.0
self.ObstaclesMsg = Obstacles()
self.ObstaclesMsg.num_obstacles = 0
self.ObstaclesMsg.hash = 0
self.ObstaclesMsg.obstacles = []
self.OdometryMsg = Odometry()
self.OdometryMsg.latitude_deg = 0
self.OdometryMsg.latitude_min = 0
self.OdometryMsg.longitude_deg = 0
self.OdometryMsg.longitude_min = 0
self.OdometryMsg.bearing_deg = 0
self.OdometryMsg.speed = 0
self.TargetMsg = Target()
self.TargetMsg.distance = 0
self.TargetMsg.bearing = 0
self.TargetListMsg = TargetList()
self.TargetListMsg.targetList = [Target(), Target()]
self.WaypointMsg = Waypoint()
self.WaypointMsg.search = False
self.WaypointMsg.gate = False
self.WaypointMsg.odom = Odometry()
class SimulatorMetaClass:
# variables defined here are common to all classes
# ideally it shouldn't matter bc we only ever need one instance
# this is bad practice imho, just defined vars in the block below
def __init__(self): # other args if ya want em
# all initially defined variables should be here
# while not technically globals, this is basically where they are
# defined for the sim, since the entire instance is the SimMetaClass
# below: class list, one class for each message type
# published or recieved. instantiate them at the bottom
# of this message definition block
# use the provided imported classes and dump these later
# you still need to set all the defaults
self.AutonStateMsg = AutonState()
self.AutonStateMsg.is_auton = False
self.CourseMsg = Course()
self.CourseMsg.num_waypoints = 0
self.CourseMsg.hash = 0
self.CourseMsg.waypoints = []
self.GPSMsg = GPS()
self.GPSMsg.latitude_deg = 39
self.GPSMsg.latitude_min = 0
self.GPSMsg.longitude_deg = -110
self.GPSMsg.longitude_min = 0
self.GPSMsg.bearing_deg = 0
self.GPSMsg.speed = -999 # this value is never used
# so it's being set to a dummy value. DO NOT USE IT
self.JoystickMsg = Joystick()
self.JoystickMsg.forward_back = 0
self.JoystickMsg.left_right = 0
self.JoystickMsg.dampen = 0
self.JoystickMsg.kill = False
self.JoystickMsg.restart = False
self.NavStatusMsg = NavStatus()
self.NavStatusMsg.nav_state = 0
self.NavStatusMsg.nav_state_name = ""
self.NavStatusMsg.completed_wps = 0
self.NavStatusMsg.missed_wps = 0
self.NavStatusMsg.total_wps = 0
self.NavStatusMsg.found_tbs = 0
self.NavStatusMsg.total_tbs = 0
self.ObstacleMsg = Obstacle()
self.ObstacleMsg.detected = False
self.ObstacleMsg.bearing = 0.0
self.ObstacleMsg.distance = 0.0
self.ObstaclesMsg = Obstacles()
self.ObstaclesMsg.num_obstacles = 0
self.ObstaclesMsg.hash = 0
self.ObstaclesMsg.obstacles = []
self.OdometryMsg = Odometry()
self.OdometryMsg.latitude_deg = 0
self.OdometryMsg.latitude_min = 0
self.OdometryMsg.longitude_deg = 0
self.OdometryMsg.longitude_min = 0
self.OdometryMsg.bearing_deg = 0
self.OdometryMsg.speed = 0
self.TargetMsg = Target()
self.TargetMsg.distance = 0
self.TargetMsg.bearing = 0
self.TargetListMsg = TargetList()
self.TargetListMsg.targetList = [Target(), Target()]
self.WaypointMsg = Waypoint()
self.WaypointMsg.search = False
self.WaypointMsg.gate = False
self.WaypointMsg.odom = Odometry()
# definitions for message processing are below, with callbacks (cb)
# at the top and publishing at the bottom
# in this setup, camelCasing denotes a class instance
# while under_scored_variables indicate a variable within the class
# to avoid confusion
def autonstate_cb(self, channel, msg):
m = AutonState.decode(msg)
self.AutonStateMsg.is_auton = m.is_auton
def course_cb(self, channel, msg):
m = Course.decode(msg)
self.CourseMsg.num_waypoints = m.num_waypoints
self.CourseMsg.hash = m.hash
self.CourseMsg.waypoints = m.waypoints
def gps_cb(self, channel, msg):
m = GPS.decode(msg)
self.GPSMsg.latitude_deg = m.latitude_deg
self.GPSMsg.latitude_min = m.latitude_min
self.GPSMsg.longitude_deg = m.longitude_deg
self.GPSMsg.longitude_min = m.longitude_min
self.GPSMsg.bearing_deg = m.bearing_deg
self.GPSMsg.speed = m.speed
def joystick_cb(self, channel, msg):
m = Joystick.decode(msg)
self.JoystickMsg.forward_back = m.forward_back
self.JoystickMsg.left_right = m.left_right
self.JoystickMsg.dampen = m.dampen
# 1-dampen/2
self.JoystickMsg.kill = m.kill
self.JoystickMsg.restart = m.restart
def navstatus_cb(self, channel, msg):
m = NavStatus.decode(msg)
self.NavStatusMsg.nav_state = m.nav_state
self.NavStatusMsg.nav_state_name = m.nav_state_name
self.NavStatusMsg.completed_wps = m.completed_wps
self.NavStatusMsg.missed_wps = m.missed_wps
self.NavStatusMsg.total_wps = m.total_wps
self.NavStatusMsg.found_tbs = m.found_tbs
self.NavStatusMsg.total_tbs = m.total_tbs
def obstacle_cb(self, channel, msg):
m = Obstacle.decode(msg)
self.ObstacleMsg.detected = m.detected
self.ObstacleMsg.bearing = m.bearing
self.ObstacleMsg.distance = m.distance
def obstacles_cb(self, channel, msg):
m = Obstacles.decode(msg)
self.ObstaclesMsg.num_obstacles = m.num_obstacles
self.ObstaclesMsg.hash = m.hash
self.ObstaclesMsg.obstacles = m.obstacles
def odometry_cb(self, channel, msg):
m = Odometry.decode(msg)
self.OdometryMsg.latitude_deg = m.latitude_deg
self.OdometryMsg.latitude_min = m.latitude_min
self.OdometryMsg.longitude_deg = m.longitude_deg
self.OdometryMsg.longitude_min = m.longitude_min
self.OdometryMsg.bearing_deg = m.bearing_deg
self.OdometryMsg.speed = m.speed
def target_cb(self, channel, msg):
m = Target.decode(msg)
self.TargetMsg.distance = m.distance
self.TargetMsg.bearing = m.bearing
def targetlist_cb(self, channel, msg):
m = TargetList.decode(msg)
self.TargetListMsg.targetList = m.targetList
def waypoint_cb(self, channel, msg):
m = Waypoint.decode(msg)
self.WaypointMsg.search = m.search
self.WaypointMsg.gate = m.gate
self.WaypointMsg.odom = m.odom
async def publish_autonstate(self, lcm):
while True:
lcm.publish("/autonstate", self.AutonStateMsg.encode())
await asyncio.sleep(1)
async def publish_course(self, lcm):
while True:
lcm.publish("/course", self.CourseMsg.encode())
await asyncio.sleep(1)
async def publish_gps(self, lcm):
while True:
lcm.publish("/gps", self.GPSMsg.encode())
await asyncio.sleep(1)
async def publish_joystick(self, lcm):
while True:
lcm.publish("/joystick", self.JoystickMsg.encode())
await asyncio.sleep(1)
async def publish_navstatus(self, lcm):
while True:
lcm.publish("/navstatus", self.NavStatusMsg.encode())
await asyncio.sleep(1)
async def publish_obstacle(self, lcm):
while True:
lcm.publish("/obstacle", self.ObstacleMsg.encode())
await asyncio.sleep(1)
async def publish_obstacles(self, lcm):
while True:
lcm.publish("/obstacles", self.ObstaclesMsg.encode())
await asyncio.sleep(1)
async def publish_odometry(self, lcm):
while True:
lcm.publish("/odometry", self.OdometryMsg.encode())
await asyncio.sleep(1)
async def publish_target(self, lcm):
while True:
lcm.publish("/target", self.TargetMsg.encode())
await asyncio.sleep(1)
async def publish_targetlist(self, lcm):
while True:
lcm.publish("/targetlist", self.TargetListMsg.encode())
await asyncio.sleep(1)
async def publish_waypoint(self, lcm):
while True:
lcm.publish("/waypoint", self.WaypointMsg.encode())
await asyncio.sleep(1)
# callback function: takes in variable from LCM, sets values locally
# SimObject definitions are below
# SimObj is the abstract base class that contains properties
# common to all objects. define additional simulator objects
# as if you would the Rover class, including proper
# superclass init
# identical to the GPS message, minus speed, bc it's a useful
# object to have internally
class AutonState:
def __init__(self, is_auton):
self.is_auton = is_auton
class Course:
def __init__(self, num_waypoints, hash, waypoints):
self.num_waypoints = num_waypoints
self.hash = hash
self.waypoints = waypoints
class GPS:
def __init__(self, latitude_deg, latitude_min, longitude_deg,
longitude_min, bearing_deg, speed):
self.latitude_deg = latitude_deg
self.latitude_min = latitude_min
self.longitude_deg = longitude_deg
self.longitude_deg = longitude_min
self.bearing = bearing_deg
self.speed = speed
class Joystick:
def __init__(self, forward_back, left_right, dampen, kill, restart):
self.forward_back = forward_back
self.left_right = left_right
self.dampen = dampen
self.kill = kill
self.restart = restart
class NavStatus:
def __init__(self, nav_state, nav_state_name, completed_wps,
missed_wps, total_wps, found_tbs, total_tbs):
self.nav_state = nav_state
self.nav_state_name = nav_state_name
self.completed_wps = completed_wps
self.missed_wps = missed_wps
self.total_wps = total_wps
self.found_tbs = found_tbs
self.total_tbs = total_tbs
class Obstacle:
def __init__(self, detected, bearing, distance):
self.detected = detected
self.bearing = bearing
self.distance = distance
class Obstacles:
def __init__(self, num_obstacles, hash, obstacles):
self.num_obstacles = num_obstacles
self.hash = hash
self.obstacles = obstacles
# pull exact coordinates from GPS
self.lat_deg = GPS.latitude_deg
self.lat_min = GPS.latitude_min
self.lon_deg = GPS.longitude_deg
self.lon_min = GPS.longitude_min
self.bearing = GPS.bearing_deg
class Odometry:
def __init__(self, latitude_deg, latitude_min, longitude_deg,
longitude_min, bearing_deg, speed):
self.latitude_deg = latitude_deg
self.latitude_min = latitude_min
self.longitude_deg = longitude_deg
self.longitude_min = longitude_min
self.bearing = bearing_deg
self.speed = speed
class Target:
def __init__(self, distance, bearing):
self.distance = distance
self.bearing = bearing
class TargetList:
def __init__(self, targetList):
self.targetList = targetList
class Waypoint:
def __init__(self, search, gate, odom):
self.search = search
self.gate = gate
self.odom = odom
# parent class of sim objects. Has all properties common to all
# objects
class SimObj(ABC):
# define initial location and other properties
def __init__(self, GPS):
self.lat_deg = GPS.latitude_deg
self.lat_min = GPS.latitude_min
self.lon_deg = GPS.longitude_deg
self.lon_min = GPS.longitude_min
self.bearing = GPS.bearing_deg
self.shape = 0 # need to create a seed system?
# any methods common to all classes should be defined
def get_coords(self):
return [self.lat_deg, self.lat_min, self.lon_deg, self.lon_min]
def get_bearing(self):
return self.bearing
# here is an abstract method, may be useful
# @abstractmethod
# def sample_abs_method(self):
# pass
class Field(SimObj):
def __init__(self, GPS, radius=2): # other properties
super().__init__(GPS)
self.radius = radius # in degrees, if not specified
# radius is 2
class Rover(SimObj):
def __init__(self, GPS, speed_trans=1, speed_rot=1):
super().__init__(GPS)
self.fov = 120 # units of degrees,
# 120 if not specified
self.cv_thresh = 5
self.speed_translational = speed_trans
# speed multiplier, 1 if not specified
self.speed_rotational = speed_rot
"""
lcm_ = aiolcm.AsyncLCM()
odom_port = os.environ.get('MROVER_ODOM_PORT', '/dev/ttyUSB0')
gps_port = os.environ.get('MROVER_GPS_PORT', '/dev/ttyUSB1')
mutex = threading.Lock()
condition = threading.Condition(mutex)
start_condition = threading.Barrier(3)
imu_ready = False
gps_ready = False
sat_count_ready = False
changed = False
gps_valid = False
msg = Odometry()
num_satellites = 0
class OdomFrame:
def __init__(self, buf):
data = struct.unpack('<fffififi??', buf)
self.roll = data[0]
self.pitch = data[1]
self.bearing = data[2]
self.lat_deg = data[3]
self.lat_min = data[4]
self.lon_deg = data[5]
self.lon_min = data[6]
self.num_sats = data[7]
self.gps_read = data[8]