async def recieve(self, lcm):
'''
Reads from the rover GPS over serial connection.
Attempts to read and proccess all supported NMEA tags
at least once before publishing a new LCM message.
Will skip publishing if the GPS quality is 0 (no fix).
Will complain (but not crash) if encountering an
unsupported message. Sleeps after publishing to
allow time for handling incoming LCM messages
'''
gps_struct = GPS()
error_counter = 0
# Mark TXT as always seen because they are not necessary
seen_tags = {tag: False if not tag == 'TXT' else True
for tag in self.NMEA_TAGS_MAPPER.keys()}
while True:
# Wait for all tags to be seen
while (not all(seen_tags.values())):
try:
msg = str(self.ser.readline())
print(msg)
error_counter = 0
except Exception as e:
if error_counter < self.max_error_count:
error_counter += 1
print(e)
await asyncio.sleep(self.sleep)
continue
else:
raise e
match_found = False
for tag, func in self.NMEA_TAGS_MAPPER.items():
if tag in msg:
match_found = True
try:
func(msg, gps_struct)
seen_tags[tag] = True
except Exception as e:
print(e)
break
if not match_found:
print('Error decoding message stream: {}'.format(msg))
# raise ValueError('Unrecognized NMEA string: {}'
# .format(msg))
# Skip publish if fix is invalid
if gps_struct.quality == 0:
print('Waiting for GPS Fix')
await(asyncio.sleep(self.sleep))
continue
lcm.publish('/gps', gps_struct.encode())
seen_tags = {tag: False if not tag == 'TXT' else True
for tag in self.NMEA_TAGS_MAPPER.keys()}
await asyncio.sleep(self.sleep)
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
Obstacles = []
Waypoints = []
Tennis_Balls = []
# 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.NavStatusMsg = NavStatus()
self.JoystickMsg = Joystick()
# self.GPSMsg = GPS()
# self.BearingMsg = Bearing()
self.GPSMsg = GPS()
self.ObstacleMsg = Obstacle()
self.TennisBallMsg = TennisBall()
self.CourseMsg = Course()
self.AutonStateMsg = AutonState()
self.NavStatusMsg.nav_state = 0
self.NavStatusMsg.completed_wps = 0
self.NavStatusMsg.missed_wps = 0
self.NavStatusMsg.total_wps = 0
self.JoystickMsg.forward_back = 0
self.JoystickMsg.left_right = 0
self.JoystickMsg.dampen = 0
self.GPSMsg.latitude_deg = 39
self.GPSMsg.latitude_min = 0
self.GPSMsg.longitude_deg = -110
self.GPSMsg.longitude_min = 0
self.GPSMsg.bearing = 0
self.GPSMsg.speed = -999 # this value is never used
# so it's being set to a dummy value. DO NOT USE IT
self.ObstacleMsg.detected = 0
self.ObstacleMsg.bearing = 0
self.TennisBallMsg.found = 0
self.TennisBallMsg.bearing = 0
self.TennisBallMsg.distance = 0
self.CourseMsg.num_waypoints = 0
self.CourseMsg.hash = 0
self.CourseMsg.waypoints = []
self.AutonStateMsg.is_auton = False
def _gpsCallback(self, channel, msg):
new_gps = GPS.decode(msg)
self.gps.update(new_gps)
# Construct filter on first GPS message
if self.filter is None and self.gps.ready():
ref_bearing = self._getFreshBearing()
if ref_bearing is None:
return
pos = self.gps.pos.asMinutes()
# vel = self.gps.vel.absolutify(ref_bearing)
vel = {"north": 0, "east": 0}
self.state_estimate = StateEstimate(
pos["lat_deg"],
pos["lat_min"],
vel["north"],
pos["long_deg"],
pos["long_min"],
vel["east"],
ref_bearing,
ref_lat=self.config["RefCoords"]["lat"],
ref_long=self.config["RefCoords"]["long"])
self._constructFilter()
self.gps.fresh = False
def _gpsCallback(self, channel, msg):
new_gps = GPS.decode(msg)
self.gps.update(new_gps)
# Attempt to filter on first GPS message
if self.filter is None and self.gps.ready():
self._constructFilter()
self.gps.fresh = False
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 gps_callback(self, channel, msg):
gps = GPS.decode(msg)
if (time.time()*1000 - self.gps_millis) > \
self.logConfig['rate_millis']['gps']:
self.write([
gps.latitude_deg, gps.latitude_min, gps.longitude_deg,
gps.longitude_min, gps.bearing_deg, gps.speed
], 'gps')
self.gps_millis = time.time() * 1000
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
gps = GPS()
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])
ns = serialPort.read_until(b',')[:-1]
latitude = latitude * -1 if ns is b'S' else latitude
longitude = float(serialPort.read_until(b',')[:-1])
ew = serialPort.read_until(b',')[:-1]
longitude = longitude * -1 if ew is b'W' else longitude
speed = float(serialPort.read_until(b',')[:-1])
bearing = float(serialPort.read_until(b',')[:-1])
gps.latitude_deg = int(latitude/100)
gps.longitude_deg = int(longitude/100)
gps.latitude_min = latitude - (
gps.latitude_deg * 100)
gps.longitude_min = longitude - (
gps.longitude_deg * 100)
gps.bearing_deg = bearing
gps.speed = speed
lcm.publish('/gps', gps.encode())
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
"""
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
Obstacles = []
Waypoints = []
Tennis_Balls = []
# 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.NavStatusMsg = NavStatus()
self.JoystickMsg = Joystick()
# self.GPSMsg = GPS()
# self.BearingMsg = Bearing()
self.GPSMsg = GPS()
self.ObstacleMsg = Obstacle()
self.TennisBallMsg = TennisBall()
self.CourseMsg = Course()
self.AutonStateMsg = AutonState()
self.NavStatusMsg.nav_state = 0
self.NavStatusMsg.completed_wps = 0
self.NavStatusMsg.missed_wps = 0
self.NavStatusMsg.total_wps = 0
self.JoystickMsg.forward_back = 0
self.JoystickMsg.left_right = 0
self.JoystickMsg.dampen = 0
self.GPSMsg.latitude_deg = 39
self.GPSMsg.latitude_min = 0
self.GPSMsg.longitude_deg = -110
self.GPSMsg.longitude_min = 0
self.GPSMsg.bearing = 0
self.GPSMsg.speed = -999 # this value is never used
# so it's being set to a dummy value. DO NOT USE IT
self.ObstacleMsg.detected = 0
self.ObstacleMsg.bearing = 0
self.TennisBallMsg.found = 0
self.TennisBallMsg.bearing = 0
self.TennisBallMsg.distance = 0
self.CourseMsg.num_waypoints = 0
self.CourseMsg.hash = 0
self.CourseMsg.waypoints = []
self.AutonStateMsg.is_auton = False
# 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 nav_test(self, channel, msg):
pass
# define this as per the spec
def nav_state_cb(self, channel, msg):
m = NavStatus.decode(msg)
self.NavStatusMsg.nav_state = m.nav_state
self.NavStatusMsg.completed_wps = m.completed_wps
self.NavStatusMsg.missed_wps = m.missed_wps
self.NavStatusMsg.total_wps = m.total_wps
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
# async def publish_bearing(self, lcm):
# while True:
# lcm.publish("\bearing", self.BearingMsg.encode())
# await asyncio.sleep(10)
async def publish_auton_state(self, lcm):
while True:
lcm.publish("\auton", self.AutonStateMsg.encode())
await asyncio.sleep(10)
# async def publish_gps_state(self, lcm):
# while True:
# lcm.publish("\GPS", self.GPSMsg.encode())
# await asyncio.sleep(10)
# bearing publish
async def publish_GPS(self, lcm):
while True:
lcm.publish("\GPS", self.GPSMsg.encode())
await asyncio.sleep(10)
async def publish_course(self, lcm):
while True:
lcm.publish("\course", self.CourseMsg.encode())
await asyncio.sleep(10)
async def publish_obstacle(self, lcm):
while True:
lcm.publish("\obstacle", self.ObstacleMsg.encode())
await asyncio.sleep(10)
async def publish_tennis_ball(self, lcm):
while True:
lcm.publish("\tennis_ball", self.TennisBallMsg.encode())
await asyncio.sleep(10)
# 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 GPS:
def __init__(self, lat0, latm0, lon0, lonm0, bearing, speed):
self.lat_deg = lat0
self.lat_min = latm0
self.lon_deg = lon0
self.lon_min = lonm0
self.bearing = bearing
self.speed = speed
# 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.lat0
self.lat_min = GPS.latm0
self.lon_deg = GPS.lon0
self.lon_min = GPS.lonm0
self.bearing = GPS.bearing0
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
class TennisBall(SimObj):
def __init__(self, GPS): # other properties
super().__init__(GPS)
self.other_prop = 0
class Obstacle(SimObj):
def __init__(self, GPS): # other properties
super().__init__(GPS)
class Waypoint(SimObj):
def __init__(self, GPS, searchable=0):
super().__init__(GPS)
self.search = searchable # defaults to false if not set
def sendTimestep(self):
# send at update rate
if (time.time() * 1000 - self.millis) < self.SEND_RATE_MILLIS:
return
imu = IMUData()
gps = GPS()
imu.accel_x_g = self.noisy['accel_x'][self.timesteps] / 9.8
imu.accel_y_g = self.noisy['accel_y'][self.timesteps] / 9.8
imu.accel_z_g = self.noisy['accel_z'][self.timesteps] / 9.8
imu.gyro_x_dps = imu.gyro_y_dps = imu.gyro_z_dps = imu.mag_x_uT = imu.mag_y_uT = \
imu.mag_z_uT = imu.roll_rad = imu.yaw_rad = 0
imu.bearing_deg = self.noisy['bearing'][self.timesteps]
imu.pitch_rad = np.radians(self.noisy['pitch'][self.timesteps])
gps.latitude_min, gps.latitude_deg = math.modf(
self.noisy['gps_north'][self.timesteps])
gps.latitude_deg = int(gps.latitude_deg)
gps.latitude_min *= 60
gps.longitude_min, gps.longitude_deg = math.modf(
self.noisy['gps_east'][self.timesteps])
gps.longitude_deg = int(gps.longitude_deg)
gps.longitude_min *= 60
gps.bearing_deg = self.noisy['bearing'][self.timesteps]
gps.speed = self.noisy['vel_total'][self.timesteps]
if gps.speed < 0:
gps.speed = 0
gps.quality = 0
if self.timesteps % self.GPS_DTS == 0:
self.lcm.publish('/gps', gps.encode())
# print('Sending GPS')
if self.timesteps % self.IMU_DTS == 0:
self.lcm.publish('/imu', imu.encode())
# print(self.timesteps / self.IMU_DTS)
# print('Sending IMU')
self.millis = time.time() * 1000
self.timesteps += 1
def _gpsCallback(self, channel, msg):
new_gps = GPS.decode(msg)
self.gps.update(new_gps)
async def sendTrajectory(self):
for point in self.trajectory:
imu_msg = IMUData()
imu_msg.accel_x_g = np.random.normal(
point[4], self.sim_config["imu_noise"]["accel_m"]) / 9.8
imu_msg.accel_y_g = 0.0
imu_msg.accel_z_g = 0.0
imu_msg.gyro_x_dps = 0.0
imu_msg.gyro_y_dps = 0.0
imu_msg.gyro_z_dps = 0.0
imu_msg.mag_x_uT = 0.0
imu_msg.mag_y_uT = 0.0
imu_msg.mag_z_uT = 0.0
imu_msg.roll_rad = 0.0
imu_msg.pitch_rad = 0.0
imu_msg.yaw_rad = 0.0
imu_msg.bearing_deg = np.random.normal(
np.degrees(point[2]),
self.sim_config["imu_noise"]["bearing_deg"])
# Clamp bearing
while imu_msg.bearing_deg < -180:
imu_msg.bearing_deg += 360
while imu_msg.bearing_deg > 180:
imu_msg.bearing_deg -= 360
self.lcm.publish(self.filter_config["imu_channel"],
imu_msg.encode())
gps_msg = GPS()
lat_m = np.random.normal(point[0],
self.sim_config["gps_noise"]["pos_m"])
lat = meters2lat(lat_m, ref_lat=self.ref_coords["lat"])
# Clamp latitude
while lat < -90:
lat += 180
while lat > 90:
lat -= 180
lon_m = np.random.normal(point[1],
self.sim_config["gps_noise"]["pos_m"])
lon = meters2lon(lon_m, lat, ref_lon=self.ref_coords["lon"])
# Clamp longitude
while lon < -180:
lon += 360
while lon > 180:
lon -= 360
gps_msg.latitude_deg, gps_msg.latitude_min = decimal2min(lat)
gps_msg.longitude_deg, gps_msg.longitude_min = decimal2min(lon)
gps_msg.bearing_deg = np.random.normal(
np.degrees(point[2]),
self.sim_config["gps_noise"]["bearing_deg"])
# Clamp bearing
while gps_msg.bearing_deg < -180:
gps_msg.bearing_deg += 360
while gps_msg.bearing_deg > 180:
gps_msg.bearing_deg -= 360
gps_msg.speed = np.random.normal(
point[3], self.sim_config["gps_noise"]["vel_m"])
gps_msg.quality = 4 if self.sim_config["rtk"] else 2
self.lcm.publish(self.filter_config["gps_channel"],
gps_msg.encode())
sensor_data = np.array(
[[0.0, lat_m, lon_m, imu_msg.bearing_deg, gps_msg.speed]])
if self.sensor_history is None:
self.sensor_history = sensor_data
else:
sensor_data[0,
0] = self.sensor_history[-1,
0] + self.sim_config["dt"]
self.sensor_history = np.vstack(
[self.sensor_history, sensor_data])
await asyncio.sleep(self.sim_config["dt"])