def test_pub_sub_msgs():
tcp = ('127.0.0.1', 9001)
pub = zmq.Pub(tcp)
sub = zmq.Sub(['test'], tcp)
msgs = [
Msgs.Vector(),
Msgs.Quaternion(),
Msgs.Array(),
Msgs.IMU(),
Msgs.Dictionary(),
Msgs.Odom(),
Msgs.Joystick(),
Msgs.Twist(),
Msgs.Wrench()
]
for tmsg in msgs:
while True:
print(tmsg)
pub.pub('test', tmsg)
topic, msg = sub.recv()
if msg:
assert msg == tmsg
assert topic == b'test'
break
def grab(self):
if not self.cam:
print('Error: camera not setup, run init() first')
return Msg.Odom()
try:
# get values from last run
pp = self.pp
focal = self.focal
p0 = self.p0
R = self.R
t = self.t
R_f = self.R_f
t_f = self.t_f
# try this???
# R = R_f.copy()
# t = np.array([0, 0, 0], dtype=np.float)
R = self.R
t = self.t
old_im = self.old_im
ret, raw = self.cam.read()
# end of video
if not ret:
print('video end')
draw(save_pts)
# break
exit()
################################################
# only for development ... delete!
# roi = (0,479,210,639)
# im = raw[roi[0]:roi[1],roi[2]:roi[3]]
im = raw
################################################
if ret:
cv2.imshow('debug', im)
cv2.waitKey(1)
# Not enough old points, p0 ... find new ones
if p0.shape[0] < 50:
print('------- reset --------')
p0 = self.featureDetection(old_im) # old_im instead?
if p0.shape[0] == 0:
print('bad image')
# continue
return
# p0 - old pts
# p1 - new pts
p0, p1 = self.featureTrack(im, old_im, p0)
# not enough new points p1 ... bad image?
if p1.shape[0] < 50:
print('------- reset p1 --------')
print('p1 size:', p1.shape)
self.old_im = im
self.p0 = p1
# continue
return
# drawKeyPoints(im, p1)
# since these are rectified images, fundatmental (F) = essential (E)
# E, mask = cv2.findEssentialMat(p0,p1,focal,pp,cv2.FM_RANSAC)
# retval, R, t, mask = cv2.recoverPose(E,p0,p1,R_f,t_f,focal,pp,mask)
E, mask = cv2.findEssentialMat(p0, p1, focal, pp, cv2.FM_RANSAC, 0.999, 1.0)
retval, R, t, mask = cv2.recoverPose(E, p0, p1, R, t, focal, pp, mask)
# print retval,R
# Now update the previous frame and previous points
# self.old_im = im.copy()
# p0 = p1.reshape(-1,1,2)
# p0 = p1
# print 'p0 size',p0.shape
# print 'p1 size',p1.shape
# print 't',t
# dt = t - t_prev
# scale = np.linalg.norm(dt)
# print scale
scale = 1.0
R_f = R.dot(R_f)
# t_f = t
t_f = t_f + scale * R_f.dot(t)
# t_prev = t
# t_f = t_f/t_f[2]
# dist += np.linalg.norm(t_f[:2])
# num = np.array([t_f[0]/t_f[2],t_f[1]/t_f[2]])
# num = t_f
print('position:', t_f)
# print 'distance:', dist
# R_f = R*R_f
# print 'R:',R_f,'t:',t_f
# print t_f
save_pts.append(t_f)
# save all
self.p0 = p1
self.R_f = R_f
self.t_f = t_f
self.old_im = im.copy()
self.t = t
self.R = R
# create message
odom = Msg.Odom()
odom['position']['position']['x'] = t_f[0]
odom['position']['position']['y'] = t_f[1]
odom['position']['position']['z'] = t_f[2]
odom['position']['orientation']['x'] = 0.0 # FIXME: 20160529 do rotation to quaternion conversion
odom['position']['orientation']['y'] = 0.0
odom['position']['orientation']['z'] = 0.0
odom['position']['orientation']['w'] = 1.0
odom['velocity']['linear']['x'] = 0.0
odom['velocity']['linear']['y'] = 0.0
odom['velocity']['linear']['z'] = 0.0
odom['velocity']['angular']['x'] = 0.0
odom['velocity']['angular']['y'] = 0.0
odom['velocity']['angular']['z'] = 0.0
return odom
except KeyboardInterrupt:
print('captured interrupt')
exit()
def run(self):
self.logger.info(
str(self.name) + '[' + str(self.pid) + '] started on' +
str(self.host) + ':' + str(self.port) + ', Daemon: ' +
str(self.daemon))
sub_imu = zmq.Sub('imu', (self.host, self.port))
sub_vo = zmq.Sub('vo', (self.host, self.port))
pub = zmq.Pub(('localhost', '9100'))
# self.logger.info('Openned camera: '+str(self.camera_num))
v = np.array([0., 0., 0.])
gps = ecef(39.0, 104.7, 1000.0)
x = np.array(gps)
q = np.array([1.0, 0., 0., 0.0])
X = np.hstack((v, x, q))
w = np.array([0, 0, 0])
self.epoch = dt.datetime.now()
# x_init = np.array([1, 1])
ekf = EKF(1, 1)
Q = np.diag([1, 2, 3, 4, 5, 6])
R = np.diag([1, 2, 3, 4, 5, 6])
ekf.init(X, kf_eqns, R, Q)
def printX(x, q):
print('------------------------------------------------')
print('pos: {:.2f} {:.2f} {:.2f}'.format(*x[0:3]))
print('vel: {:.2f} {:.2f} {:.2f}'.format(*x[3:6]))
print('qua: {:.2f} {:.2f} {:.2f} {:.2f}'.format(*x[6:]))
print('qu2: {:.2f} {:.2f} {:.2f} {:.2f}'.format(*q))
try:
# ans = {}
while True:
topic, imu = sub_imu.recv()
if imu:
# print(imu)
# get this from imu
x = imu['linear_acceleration']['x']
y = imu['linear_acceleration']['y']
z = imu['linear_acceleration']['z']
f = np.array([x, y, z])
x = imu['angular_velocity']['x']
y = imu['angular_velocity']['y']
z = imu['angular_velocity']['z']
w = np.array([x, y, z])
qw = imu['orientation']['w']
qx = imu['orientation']['x']
qy = imu['orientation']['y']
qz = imu['orientation']['z']
# integrate navigation EoM
u = np.hstack((f, w))
X = self.eom.step(X, u)
printX(X, [qw, qx, qy, qz])
topic, vo = sub_vo.recv()
if vo:
# self.kf.predict()
# self.kf.??
print('wtf ... not implemented yet')
else:
vo = None
if 0:
u = np.array([0, 0])
z = np.array([0, 0])
ekf.predict(u)
ekf.update(z)
# xar.append(X[3])
# yar.append(X[4])
# plot(fig, li, xx, yy)
# [odom]-----------------------------------------------------
# pose(position[vector], orientation[quaternion])
# twist(linear[vector], angular[vector])
odom = msg.Odom()
odom['position']['position']['x'] = X[3]
odom['position']['position']['y'] = X[4]
odom['position']['position']['z'] = X[5]
odom['position']['orientation']['x'] = X[6]
odom['position']['orientation']['y'] = X[7]
odom['position']['orientation']['z'] = X[8]
odom['position']['orientation']['z'] = X[9]
odom['velocity']['linear']['x'] = X[0]
odom['velocity']['linear']['y'] = X[1]
odom['velocity']['linear']['z'] = X[2]
odom['velocity']['angular']['x'] = w[0]
odom['velocity']['angular']['y'] = w[1]
odom['velocity']['angular']['z'] = w[2]
# print(odom)
pub.pub('/nav', odom)
# time.sleep(0.05)
except KeyboardInterrupt:
print('Navigation: shutting down ...')