def points_in_car_frame(self, lat, lon, heading):
lc = LocalCoord.from_geodetic([lat, lon, 0.])
# Build rotation matrix
heading = math.radians(-heading + 90)
c, s = np.cos(heading), np.sin(heading)
rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
# Convert to local coordinates
points_carframe = lc.geodetic2ned(self.points).T
# Rotate with heading of car
points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T
return points_carframe
def mpc_vwr_thread(addr="127.0.0.1"):
plt.ion()
fig = plt.figure(figsize=(15, 15))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlim([-SCALE, SCALE])
ax.set_ylim([-SCALE, SCALE])
ax.grid(True)
line, = ax.plot([0.0], [0.0], ".b")
line2, = ax.plot([0.0], [0.0], 'r')
ax.set_aspect('equal', 'datalim')
plt.show()
live_location = messaging.sub_sock('liveLocation',
addr=addr,
conflate=True)
gps_planner_points = messaging.sub_sock('gpsPlannerPoints', conflate=True)
gps_planner_plan = messaging.sub_sock('gpsPlannerPlan', conflate=True)
last_points = messaging.recv_one(gps_planner_points)
last_plan = messaging.recv_one(gps_planner_plan)
while True:
p = messaging.recv_one_or_none(gps_planner_points)
pl = messaging.recv_one_or_none(gps_planner_plan)
ll = messaging.recv_one(live_location).liveLocation
if p is not None:
last_points = p
if pl is not None:
last_plan = pl
if not last_plan.gpsPlannerPlan.valid:
time.sleep(0.1)
line2.set_color('r')
continue
p0 = last_points.gpsPlannerPoints.points[0]
p0 = np.array([p0.x, p0.y, p0.z])
n = LocalCoord.from_geodetic(np.array([ll.lat, ll.lon, ll.alt]))
points = []
print(len(last_points.gpsPlannerPoints.points))
for p in last_points.gpsPlannerPoints.points:
ecef = np.array([p.x, p.y, p.z])
points.append(n.ecef2ned(ecef))
points = np.vstack(points)
line.set_xdata(points[:, 1])
line.set_ydata(points[:, 0])
y = np.matrix(np.arange(-100, 100.0, 0.5))
x = -np.matrix(np.polyval(last_plan.gpsPlannerPlan.poly, y))
xy = np.hstack([x.T, y.T])
cur_heading = np.radians(ll.heading - 90)
c, s = np.cos(cur_heading), np.sin(cur_heading)
R = np.array([[c, -s], [s, c]])
xy = xy.dot(R)
line2.set_xdata(xy[:, 1])
line2.set_ydata(-xy[:, 0])
line2.set_color('g')
ax.set_xlim([-SCALE, SCALE])
ax.set_ylim([-SCALE, SCALE])
fig.canvas.draw()
fig.canvas.flush_events()