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
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"])