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 pygecko_image():
frame = np.random.randint(0, 255, size=(640, 480))
jpeg = cv2.imencode('.jpg', frame)[1]
b64 = base64.b64encode(jpeg)
m = Msg.Image(b64)
m = Msg.serialize(m)
m = Msg.deserialize(m)
# print m
ii = base64.b64decode(m['image'])
ii = np.fromstring(ii, dtype=np.uint8)
cv2.imdecode(ii, 0)
def Chaos_Func():
print('CHAOS BEGINS !!!!!!!!!!!!!!!!!!!!!!!!!!!!')
fs = FileStorage()
fs.readJson("net.json")
net = fs.db
# pprint(self.db)
# self.play(self.db['start'])
fs.db = {}
fs.readJson("clips.json")
clips = fs.db
# subs = []
# for sub in net:
# ip = net[sub]
# s = zmq.Pub(bind_to=(ip[0], ip[1]))
# subs.append(s)
# print(' >> Subscribed to:', sub, '@', ip)
subs = {}
subs['sounds'] = zmq.Pub(bind_to=('localhost', 9000))
subs['speak'] = subs['sounds']
subs['servos'] = zmq.Pub(bind_to=('localhost', 9004))
while True:
for key in subs.keys():
if key == 'sounds':
clip = random.choice(clips.keys())
msg = Messages.Dictionary()
msg.dict['sound'] = clip
subs['sounds'].pub('sounds', msg)
# print('msg:', msg)
time.sleep(7)
elif key == 'speak':
word = getnrandom()
msg = Messages.Dictionary()
msg.dict[key] = word
subs[key].pub(key, msg)
time.sleep(4)
elif key == 'servos':
num = random.choice(range(3))
name = 'door{}'.format(num)
msg = Messages.Dictionary()
msg.dict['name'] = name
msg.dict['angle'] = random.choice(range(30, 150, 10))
subs[key].pub(key, msg)
time.sleep(1)
else:
print('<<< Oops, unknown key:', key, '>>>')
def run(self):
print('start pub')
while True:
# msg = Msg.Array()
# msg.append(1)
# msg.append(2)
msg = Msg.Vector()
msg.set(1, 2, 3)
self.pub.pub('a', msg)
print(msg)
msg = Msg.Dictionary()
msg.dict['bob'] = 1
self.pub.pub('b', msg)
print(msg)
time.sleep(1)
def run(self):
# self.logger.info(str(self.name) + '[' + str(self.pid) + '] started on ' + str(self.host) + ':' + str(self.port) + ', Daemon: ' + str(self.daemon))
# pub = zmq.PubBase64((self.host, self.port))
pub = zmq.Pub((self.host, self.port), hwm=100)
camera = Camera('pi')
# camera.init(cameraNumber=self.camera_num, win=(640, 480))
camera.init(win=(640, 480))
# self.logger.info('Openned camera: ' + str(self.camera_num))
try:
while True:
ret, frame = camera.read()
# jpeg = cv2.imencode('.jpg', frame)[1] # jpeg compression
# msg = Msg.Image(jpeg)
if ret:
msg = Msg.Image()
msg.img = frame
pub.pub('image_color', msg)
# print '[*] frame: %d k jpeg: %d k'%(frame.size/1000,len(jpeg)/1000)
# time.sleep(0.01)
else:
print('dropped image')
except KeyboardInterrupt:
print('Ctl-C ... exiting')
return
def HW_Func():
status = LogicFunctionDisplay([0x70], 1)
psf = LogicFunctionDisplay([0x71, 0x72])
psb = LogicFunctionDisplay([0x73, 0x74, 0x75])
# psb.setBrightness(7) # can be a value between [off] 0-15 [brightest]
imu = IMU() # inerial measurement unit
sub = zmq.Sub(['servos'], ('localhost', 9004))
js_sub = zmq.Sub(['cmd'], ('localhost', 9006))
# you can monitor the publishing of this with:
# topic_echo.py localhost 9005 imu
pub = zmq.Pub(('localhost', 9005))
# create servos
servos = {}
servos['door0'] = Servo(0)
servos['door1'] = Servo(1)
servos['door2'] = Servo(2)
servos['door3'] = Servo(3)
servos['door4'] = Servo(4)
servos['js'] = Servo(7) # this is just for demo
pprint(servos)
while True:
status.update()
psf.update()
psb.update()
time.sleep(0.5)
accel, mag, gyro = imu.get()
msg = Msg.IMU()
msg.linear_acceleration.set(*accel)
# msg.angular_velocity.set(*gyro)
msg.orientation.set(1, 0, 0, 0)
pub.pub('imu', msg)
topic, msg = sub.recv()
if msg:
if topic == 'servos':
print('Topic, Msg:', topic, msg)
angle = msg['angle']
name = msg['name']
servos[name].angle = angle
elif topic == 'cmd':
print('<<< crap cmd in wrong place >>>')
# this is a joystick test
topic, msg = js_sub.recv()
if msg:
if topic == 'cmd':
print('Topic, Msg:', topic, msg)
angle = 90 * msg.angular.x + 90
servos['js'].angle = angle
time.sleep(0.025)
def read_ir(self):
mcp = self.mcp
adc = [0] * 8
for i in range(8):
adc[i] = mcp.read_adc(i)
msg = Msg.Range()
msg.fov = 20.0
msg.range = adc
return msg
def Pub():
pub = zmq.Pub(('127.0.0.1', 9000))
print('start pub')
cnt = 0
while True:
# msg = Msg.Array()
# msg.append(1)
# msg.append(2)
msg = Msg.Vector()
cnt += 1
msg.set(1, 2, cnt)
pub.pub('a', msg)
# print('>>', msg)
msg = Msg.Dictionary()
msg.dict['bob'] = 1
pub.pub('b', msg)
# print('>>', msg)
time.sleep(.1)
def run(self):
print('Publishing ball {}:{}'.format('0.0.0.0', '9000'))
pub_ball = zmq.Pub(('0.0.0.0', 9000))
print('Publishing image_color {}:{}'.format('0.0.0.0', '9010'))
pub_image = zmq.Pub(('0.0.0.0', 9010))
try:
while True:
ret, frame = self.camera.read()
msg = Msg.Image()
msg.img = frame
pub_image.pub('image_color', msg)
width, height = frame.shape[:2]
center, radius = self.balltracker.find(frame)
if center and radius > 10:
x, y = center
xx = x - width / 2
yy = y - height / 2
msg = Msg.Vector()
msg.set(xx, yy, 0)
pub_ball.pub('ball', msg)
faces = self.face.find(frame)
if len(faces) > 0:
print('found a face!', faces, type(faces))
# msg = Msg.Array()
# msg.array = [[1,2,3,4], [4,5,6,7]]
# msg.array = list(faces)
# msg.array = faces
# print('msg >>', msg, type(msg.array))
# pub_ball.pub('faces', msg)
# print(msg)
# sleep(0.01)
except KeyboardInterrupt:
print('Ctl-C ... exiting')
return
def read_compass(self):
# read ahrs ---------
roll, pitch, heading = self.ahrs.read(deg=True)
msg = Msg.Compass(units=Msg.Compass.COMPASS_DEGREES)
msg.roll = roll
msg.pitch = pitch
msg.heading = heading
if (-90.0 > roll > 90.0) or (-90.0 > pitch > 90.0):
print('Crap we flipped!!!', msg)
return msg
def test():
msg = Msg.Vector()
msg.set(1, 2, 3)
pub_thread = threading.Thread(target=publisher, args=(msg, ))
pub_thread.daemon = True
pub_thread.start()
sub_thread = threading.Thread(target=subscriber, args=(msg, ))
sub_thread.daemon = True
sub_thread.start()
pub_thread.join()
sub_thread.join()
time.sleep(0.1)
def test_bag():
bag = Bag()
bag.open('imu')
num_msg = 105
for i in range(0, num_msg):
msg = Msg.Vector()
msg.set(1, 2, 3)
bag.push(msg)
bag.close()
filename = 'imu.bag'
ans = bag.readFromFile(filename)
os.remove(filename)
# print 'Found {} messages in file {}'.format(len(ans), filename)
# print 'type:', type(ans[0])
# print ans[0]
assert len(ans) == num_msg
assert isinstance(ans[0], Msg.Vector)
def __init__(self, net, topic='cmd'):
# mp.Process.__init__(self)
self.pub = zmq.Pub(net)
self.topic = topic
# init SDL2 and grab joystick
sdl2.SDL_Init(sdl2.SDL_INIT_JOYSTICK)
self.js = sdl2.SDL_JoystickOpen(0)
# grab info for display
a = sdl2.SDL_JoystickNumAxes(self.js)
b = sdl2.SDL_JoystickNumButtons(self.js)
h = sdl2.SDL_JoystickNumHats(self.js)
self.old_twist = Msg.Twist()
print('==========================================')
print(' Joystick ')
print(' axes:', a, 'buttons:', b, 'hats:', h)
print(' publishing: {}:{}'.format(net[0], net[1], topic))
print('==========================================')
def run(self):
pub = Zmq.Pub(self.addr, hwm=100)
twist = Msg.Twist()
# fd = sys.stdin.fileno()
# tty.setraw(sys.stdin.fileno())
while True:
# have to do some fancy stuff to avoid sending \n all the time
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(fd)
key = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
print('>>>', key)
if key == 'a':
twist.angular.z += 0.1
twist.angular.z = limit_max(twist.angular.z)
elif key == 'd':
twist.angular.z -= 0.1
twist.angular.z = limit_min(twist.angular.z)
elif key == 'w':
twist.linear.x += 0.1
twist.linear.x = limit_max(twist.linear.x)
elif key == 'x':
twist.linear.x -= 0.1
twist.linear.x = limit_min(twist.linear.x)
elif key == 's': # stop - all 0's
twist.linear.set(0.0, 0.0, 0.0)
twist.angular.set(0.0, 0.0, 0.0)
elif key == 'q':
exit()
pub.pub('twist_kb', twist)
def run(self, verbose, rate):
js = self.js
dt = 1.0/rate
ps4 = Msg.Joystick()
while True:
try:
sdl2.SDL_JoystickUpdate()
# left axis
x = sdl2.SDL_JoystickGetAxis(js, 0) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 1) / 32768
ps4.axes.leftStick = [x, y]
# right axis
x = sdl2.SDL_JoystickGetAxis(js, 2) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 5) / 32768
ps4.axes.rightStick = [x, y]
# other axes
ps4.axes.L2 = sdl2.SDL_JoystickGetAxis(js, 3) / 32768
ps4.axes.R2 = sdl2.SDL_JoystickGetAxis(js, 4) / 32768
# accels
x = sdl2.SDL_JoystickGetAxis(js, 6) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 7) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 8) / 32768
ps4.axes.accels = [x, y, z]
# gyros
x = sdl2.SDL_JoystickGetAxis(js, 9) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 10) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 11) / 32768
ps4.axes.gyros = [x, y, z]
# get buttons
ps4.buttons.s = sdl2.SDL_JoystickGetButton(js, 0)
ps4.buttons.x = sdl2.SDL_JoystickGetButton(js, 1)
ps4.buttons.o = sdl2.SDL_JoystickGetButton(js, 2)
ps4.buttons.t = sdl2.SDL_JoystickGetButton(js, 3)
ps4.buttons.L1 = sdl2.SDL_JoystickGetButton(js, 4)
ps4.buttons.R1 = sdl2.SDL_JoystickGetButton(js, 5)
ps4.buttons.L2 = sdl2.SDL_JoystickGetButton(js, 6)
ps4.buttons.R2 = sdl2.SDL_JoystickGetButton(js, 7)
ps4.buttons.share = sdl2.SDL_JoystickGetButton(js, 8)
ps4.buttons.options = sdl2.SDL_JoystickGetButton(js, 9)
ps4.buttons.L3 = sdl2.SDL_JoystickGetButton(js, 10)
ps4.buttons.R3 = sdl2.SDL_JoystickGetButton(js, 11)
ps4.buttons.ps = sdl2.SDL_JoystickGetButton(js, 12)
ps4.buttons.pad = sdl2.SDL_JoystickGetButton(js, 13)
# get hat
# [up right down left] = [1 2 4 8]
ps4.buttons.hat = sdl2.SDL_JoystickGetHat(js, 0)
# print('b 12', sdl2.SDL_JoystickGetButton(js, 12))
# print('b 13', sdl2.SDL_JoystickGetButton(js, 13))
if verbose: print(Msg.Joystick.screen(ps4))
self.pub.pub('js', ps4)
time.sleep(dt)
except (IOError, EOFError):
print('[-] Connection gone .... bye')
break
# except Exception as e:
# print('Ooops:', e)
# else:
# raise Exception('Joystick: Something bad happened!')
# clean-up
sdl2.SDL_JoystickClose(js)
print('Bye ...')
def read(self, verbose=False):
js = self.js
dt = self.sleep
ps4 = Msg.Joystick()
twist = Msg.Twist()
try:
sdl2.SDL_JoystickUpdate()
# left axis
x = sdl2.SDL_JoystickGetAxis(js, 0) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 1) / 32768
ps4.axes.leftStick = [x, y]
# right axis
x = sdl2.SDL_JoystickGetAxis(js, 2) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 5) / 32768
ps4.axes.rightStick = [x, y]
# other axes
ps4.axes.L2 = sdl2.SDL_JoystickGetAxis(js, 3) / 32768
ps4.axes.R2 = sdl2.SDL_JoystickGetAxis(js, 4) / 32768
# accels
x = sdl2.SDL_JoystickGetAxis(js, 6) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 7) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 8) / 32768
ps4.axes.accels = [x, y, z]
# gyros
x = sdl2.SDL_JoystickGetAxis(js, 9) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 10) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 11) / 32768
ps4.axes.gyros = [x, y, z]
# get buttons
ps4.buttons.s = sdl2.SDL_JoystickGetButton(js, 0)
ps4.buttons.x = sdl2.SDL_JoystickGetButton(js, 1)
ps4.buttons.o = sdl2.SDL_JoystickGetButton(js, 2)
ps4.buttons.t = sdl2.SDL_JoystickGetButton(js, 3)
ps4.buttons.L1 = sdl2.SDL_JoystickGetButton(js, 4)
ps4.buttons.R1 = sdl2.SDL_JoystickGetButton(js, 5)
ps4.buttons.L2 = sdl2.SDL_JoystickGetButton(js, 6)
ps4.buttons.R2 = sdl2.SDL_JoystickGetButton(js, 7)
ps4.buttons.share = sdl2.SDL_JoystickGetButton(js, 8)
ps4.buttons.options = sdl2.SDL_JoystickGetButton(js, 9)
ps4.buttons.L3 = sdl2.SDL_JoystickGetButton(js, 10)
ps4.buttons.R3 = sdl2.SDL_JoystickGetButton(js, 11)
ps4.buttons.ps = sdl2.SDL_JoystickGetButton(js, 12)
ps4.buttons.pad = sdl2.SDL_JoystickGetButton(js, 13)
# get hat
# [up right down left] = [1 2 4 8]
ps4.buttons.hat = sdl2.SDL_JoystickGetHat(js, 0)
# print('b 12', sdl2.SDL_JoystickGetButton(js, 12))
# print('b 13', sdl2.SDL_JoystickGetButton(js, 13))
#self.pub.pub('js', ps4)
x, y = ps4.axes.rightStick
twist.linear.set(x, y, 0)
x, y = ps4.axes.leftStick
twist.angular.set(x, y, 0)
# verbose = True
if verbose:
print(twist)
if (self.old_twist.angular
== twist.angular) and (self.old_twist.linear
== twist.linear):
# print('js no msg')
pass
else:
self.pub.pub(self.topic, twist)
self.old_twist = twist
# print('js message sent')
time.sleep(dt)
except (IOError, EOFError):
print('[-] Connection gone .... bye')
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 ...')
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()