def ecef_euler_from_ned(ned_ecef_init, ned_pose): ''' Got it from here: Using Rotations to Build Aerospace Coordinate Systems -Don Koks ''' converter = LocalCoord.from_ecef(ned_ecef_init) x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0]) y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0]) z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0]) x1 = rot(z0, ned_pose[2]).dot(x0) y1 = rot(z0, ned_pose[2]).dot(y0) z1 = rot(z0, ned_pose[2]).dot(z0) x2 = rot(y1, ned_pose[1]).dot(x1) y2 = rot(y1, ned_pose[1]).dot(y1) z2 = rot(y1, ned_pose[1]).dot(z1) x3 = rot(x2, ned_pose[0]).dot(x2) y3 = rot(x2, ned_pose[0]).dot(y2) #z3 = rot(x2, ned_pose[0]).dot(z2) x0 = array([1, 0, 0]) y0 = array([0, 1, 0]) z0 = array([0, 0, 1]) psi = np.arctan2(inner(x3, y0), inner(x3, x0)) theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2)) y2 = rot(z0, psi).dot(y0) z2 = rot(y2, theta).dot(z0) phi = np.arctan2(inner(y3, z2), inner(y3, y2)) ret = array([phi, theta, psi]) return ret
def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
'''
Got the math from here:
Using Rotations to Build Aerospace Coordinate Systems
-Don Koks
Also accepts array of ecef_poses and array of ned_ecef_inits.
Where each row is a pose and an ecef_init.
'''
ned_ecef_init = array(ned_ecef_init)
ecef_poses = array(ecef_poses)
output_shape = ecef_poses.shape
ned_ecef_init = np.atleast_2d(ned_ecef_init)
if ned_ecef_init.shape[0] == 1:
ned_ecef_init = np.tile(ned_ecef_init[0], (output_shape[0], 1))
ecef_poses = np.atleast_2d(ecef_poses)
ned_poses = np.zeros(ecef_poses.shape)
for i, ecef_pose in enumerate(ecef_poses):
converter = LocalCoord.from_ecef(ned_ecef_init[i])
x0 = array([1, 0, 0])
y0 = array([0, 1, 0])
z0 = array([0, 0, 1])
x1 = rot(z0, ecef_pose[2]).dot(x0)
y1 = rot(z0, ecef_pose[2]).dot(y0)
z1 = rot(z0, ecef_pose[2]).dot(z0)
x2 = rot(y1, ecef_pose[1]).dot(x1)
y2 = rot(y1, ecef_pose[1]).dot(y1)
z2 = rot(y1, ecef_pose[1]).dot(z1)
x3 = rot(x2, ecef_pose[0]).dot(x2)
y3 = rot(x2, ecef_pose[0]).dot(y2)
#z3 = rot(x2, ecef_pose[0]).dot(z2)
x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
psi = np.arctan2(inner(x3, y0), inner(x3, x0))
theta = np.arctan2(-inner(x3, z0),
np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
y2 = rot(z0, psi).dot(y0)
z2 = rot(y2, theta).dot(z0)
phi = np.arctan2(inner(y3, z2), inner(y3, y2))
ned_poses[i] = array([phi, theta, psi])
return ned_poses.reshape(output_shape)
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()
