You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
47 lines
1.6 KiB
47 lines
1.6 KiB
6 years ago
|
# mock_gps.py: Publishes a generated path moving at 15m/s to gpsLocation
|
||
|
|
# USAGE: python mock_gps.py
|
||
|
|
# Then start manager
|
||
|
|
|
||
|
|
from itertools import cycle
|
||
|
|
import time
|
||
|
|
import zmq
|
||
|
|
|
||
|
|
from cereal import log
|
||
|
|
import cereal.messaging as messaging
|
||
|
|
from cereal.services import service_list
|
||
|
|
|
||
|
|
degrees_per_meter = 0.000009000009 # approximation
|
||
|
|
start_lat = 43.64199141443989
|
||
|
|
start_lng = -94.97520411931725
|
||
|
|
|
||
|
|
def gen_path(length_seconds, speed=15):
|
||
|
|
return [{"lat": start_lat,
|
||
|
|
"lng": start_lng + speed * i * degrees_per_meter, # moving along longitudinal axis at speed m/s
|
||
|
|
"speed": speed}
|
||
|
|
for i in range(1, length_seconds + 1)]
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
gpsLocation = messaging.pub_sock('gpsLocation')
|
||
|
|
|
||
|
|
path_stopped_5s = [{"lat": start_lat, "lng": start_lng, "speed": 0}] * 5
|
||
|
|
path_moving = gen_path(30, speed=15)
|
||
|
|
path_stopped_5s_then_moving = path_stopped_5s + path_moving
|
||
|
|
|
||
|
|
for point in cycle(path_stopped_5s_then_moving):
|
||
|
|
print('sending gpsLocation from point: {}'.format(str(point)))
|
||
|
|
dat = messaging.new_message()
|
||
|
|
dat.init('gpsLocation')
|
||
|
|
dat.gpsLocation.latitude = point['lat']
|
||
|
|
dat.gpsLocation.longitude = point['lng']
|
||
|
|
dat.gpsLocation.speed = point['speed']
|
||
|
|
dat.gpsLocation.flags = 0
|
||
|
|
dat.gpsLocation.altitude = 0
|
||
|
|
dat.gpsLocation.bearing = 0 # todo we can mock this
|
||
|
|
dat.gpsLocation.accuracy = 1
|
||
|
|
dat.gpsLocation.timestamp = int(time.time() * 1000)
|
||
|
|
dat.gpsLocation.source = log.GpsLocationData.SensorSource.android
|
||
|
|
|
||
|
|
gpsLocation.send(dat.to_bytes())
|
||
|
|
time.sleep(1)
|
||
|
|
|