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.
69 lines
2.0 KiB
69 lines
2.0 KiB
5 years ago
|
#!/usr/bin/env python
|
||
|
|
|
||
|
|
import matplotlib
|
||
|
|
matplotlib.use('TkAgg')
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
import zmq
|
||
|
|
from cereal.services import service_list
|
||
|
|
from selfdrive.config import Conversions as CV
|
||
|
|
import cereal.messaging as messaging
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
live_map_sock = messaging.sub_sock(service_list['liveMapData'].port, conflate=True)
|
||
|
|
plan_sock = messaging.sub_sock(service_list['plan'].port, conflate=True)
|
||
|
|
|
||
|
|
plt.ion()
|
||
|
|
fig = plt.figure(figsize=(8, 16))
|
||
|
|
ax = fig.add_subplot(2, 1, 1)
|
||
|
|
ax.set_title('Map')
|
||
|
|
|
||
|
|
SCALE = 1000
|
||
|
|
ax.set_xlim([-SCALE, SCALE])
|
||
|
|
ax.set_ylim([-SCALE, SCALE])
|
||
|
|
ax.set_xlabel('x [m]')
|
||
|
|
ax.set_ylabel('y [m]')
|
||
|
|
ax.grid(True)
|
||
|
|
|
||
|
|
points_plt, = ax.plot([0.0], [0.0], "--xk")
|
||
|
|
cur, = ax.plot([0.0], [0.0], "xr")
|
||
|
|
|
||
|
|
speed_txt = ax.text(-500, 900, '')
|
||
|
|
curv_txt = ax.text(-500, 775, '')
|
||
|
|
|
||
|
|
ax = fig.add_subplot(2, 1, 2)
|
||
|
|
ax.set_title('Curvature')
|
||
|
|
curvature_plt, = ax.plot([0.0], [0.0], "--xk")
|
||
|
|
ax.set_xlim([0, 500])
|
||
|
|
ax.set_ylim([0, 1e-2])
|
||
|
|
ax.set_xlabel('Distance along path [m]')
|
||
|
|
ax.set_ylabel('Curvature [1/m]')
|
||
|
|
ax.grid(True)
|
||
|
|
|
||
|
|
plt.show()
|
||
|
|
|
||
|
|
while True:
|
||
|
|
m = messaging.recv_one_or_none(live_map_sock)
|
||
|
|
p = messaging.recv_one_or_none(plan_sock)
|
||
|
|
if p is not None:
|
||
|
|
v = p.plan.vCurvature * CV.MS_TO_MPH
|
||
|
|
speed_txt.set_text('Desired curvature speed: %.2f mph' % v)
|
||
|
|
|
||
|
|
if m is not None:
|
||
|
|
print("Current way id: %d" % m.liveMapData.wayId)
|
||
|
|
curv_txt.set_text('Curvature valid: %s Dist: %03.0f m\nSpeedlimit valid: %s Speed: %.0f mph' %
|
||
|
|
(str(m.liveMapData.curvatureValid),
|
||
|
|
m.liveMapData.distToTurn,
|
||
|
|
str(m.liveMapData.speedLimitValid),
|
||
|
|
m.liveMapData.speedLimit * CV.MS_TO_MPH))
|
||
|
|
|
||
|
|
points_plt.set_xdata(m.liveMapData.roadX)
|
||
|
|
points_plt.set_ydata(m.liveMapData.roadY)
|
||
|
|
curvature_plt.set_xdata(m.liveMapData.roadCurvatureX)
|
||
|
|
curvature_plt.set_ydata(m.liveMapData.roadCurvature)
|
||
|
|
|
||
|
|
fig.canvas.draw()
|
||
|
|
fig.canvas.flush_events()
|