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 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 test_pub_sub_vector():
tcp = ('127.0.0.1', 9001)
pub = zmq.Pub(tcp)
sub = zmq.Sub(['test'], tcp)
d = {'Class': 'Vector', 'x': 1.0, 'z': 2.0}
tmsg = dict_to_class(d)
for _ in range(10):
pub.pub('test', tmsg)
topic, msg = sub.recv()
if msg:
assert msg == tmsg
assert topic == b'test'
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 test_pub_sub():
tcp = ('127.0.0.1', 9000)
pub = zmq.Pub(tcp)
sub = zmq.Sub(['test'], tcp)
# tmsg = {'a': 1, 'b': 2}
tmsg = Vector()
tmsg.set(2, 3, 4)
while True:
pub.pub('test', tmsg)
topic, msg = sub.recv()
if msg:
assert msg == tmsg
assert topic == b'test'
break
def Sub():
sub = zmq.Sub(['a', 'b'], ('127.0.0.1', 9000))
print('start sub')
while True:
topic, msg = sub.recv()
print('<<', topic, msg)
time.sleep(2)
def publisher(msg):
# Prepare publisher
pub = Zmq.Pub(bind_to=('localhost', 9000))
for i in range(7):
pub.pub('test', msg)
time.sleep(0.1)
def soccer():
md_pins = [
17,
18, # m0 pwm a, b
22,
23, # m1 pwm a, b
24,
25, # m0 a, b
26,
27 # m1 a, b
]
robot = SoccerRobot(md_pins)
cmd_sub = zmq.Sub(topics=['cmds'], connect_to=('0.0.0.0', 9000))
# telemetry_pub = zmq.Pub(bind_to=('0.0.0.0', 9010))
while True:
robot.loop_once()
# get info
topic, msg = cmd_sub.recv()
if msg:
print('msg:', topic, msg)
if topic == 'quit':
self.shutdown()
break
elif topic == 'cmd':
# need logic for command
cmd = self.makeCmd(msg)
self.md.setMotors(*cmd)
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 run(self):
self.sub = zmq.Sub(['voice'])
while True:
time.sleep(3)
msg = self.sub.recv()
if msg:
print(msg)
def test_pub_sub_b64():
tcp = ('127.0.0.1', 9002)
pub = zmq.Pub(tcp)
sub = zmq.Sub(['test'], tcp)
im = np.random.rand(100, 100)
tmsg = Image()
tmsg.img = im
# print(tmsg['size'], tmsg['depth'])
while True:
pub.pub('test', tmsg)
topic, msg = sub.recv()
print('topic?', topic)
if msg:
if tmsg.b64:
tmsg.decodeB64()
assert msg.img.shape == tmsg.img.shape
assert msg.img.all() == tmsg.img.all()
assert topic == b'test'
break
def subscriber(msg):
# Subscribe to everything
sub = Zmq.Sub('test', connect_to=('localhost', 9000))
# Get and process messages
for i in range(7):
tp, ret = sub.recv()
if ret:
assert ret == msg
print('found:', ret == msg)
return
time.sleep(0.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 __init__(self, host, port):
self.pub = zmq.Pub((host, port))
# 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)
print('==========================================')
print(' Joystick ')
print(' axes:', a, 'buttons:', b, 'hats:', h)
print(' publishing: {}:{}'.format(host, port))
print('==========================================')
def Sound_Func():
print('sounds starts')
host = ('localhost', 9000)
# get sound clips
fs = FileStorage()
fs.readJson("clips.json")
db = fs.db
# pprint(self.db)
# self.play(self.db['start'])
print('Found {} sounds clips'.format(len(db)))
# pub/sub setup
sub = zmq.Sub(['sounds', 'speak'], host)
audio_player = AudioPlayer()
audio_player.set_volume(15) # volume is 0-100%
time.sleep(0.1)
print('AudioPlayer found:', audio_player.audio_player)
r2 = TTAstromech()
r2.speak('warning')
time.sleep(0.5)
print('------start--------')
while True:
topic, msg = sub.recv()
# print('msg?')
if msg:
if topic == 'sounds':
print('Topic, Msg:', topic, msg)
# print('play sound')
key = msg.dict['sound']
if key in db:
audio_player.play('clips/' + db[key])
else:
print('ERROR: key not found in db:', key)
elif topic == 'speak':
print('Topic, Msg:', topic, msg)
word = msg.dict['speak']
word = word[0:6] # limit R2 to 6 letters
r2.speak(word)
# else:
# print('nothing')
time.sleep(0.5)
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, topic, hostinfo):
print('Connecting to: {}[{}]:{}'.format(hostinfo[0], socket.gethostbyname(hostinfo[0]), hostinfo[1]))
print('Using topic: {}'.format(topic))
print('Press "q" to quit')
s = zmq.Sub(topics=topic, connect_to=hostinfo, hwm=100)
while True:
try:
tp, msg = s.recv()
if msg:
# print('Image:', msg)
im = msg.img
cv2.imshow('Camera', im)
key = cv2.waitKey(1)
if key == ord('q'):
break
except (IOError, EOFError):
print('[-] Connection gone .... bye')
break
def run(self):
smc = SMC('/dev/tty.usbserial0')
smc.init()
smc.stop() # make sure we are stopped?
saber = Sabertooth('/dev/tty.usbserial1')
saber.stop() # make sure we are stopped?
# pub = zmq.Pub() # do i need to feedback motor errors?
sub = zmq.Sub(['cmd', 'dome'])
while True:
topic, msg = sub.recv()
if msg:
if topic == 'cmd':
print('got cmd')
elif topic == 'dome':
print('got dome')
else:
time.sleep(0.5)
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):
self.logger.info(str(self.name) + '[' + str(self.pid) + '] started on' + str(self.host) + ':' + str(self.port) + ', Daemon: ' + str(self.daemon))
pub = Zmq.Pub((self.host, self.port))
params = {
'pp': (240, 220),
'focallength': 200
}
vo = VideoOdom()
if self.camera is None: vo.init(params)
else: vo.init(params, self.camera)
try:
while True:
odom = vo.grab()
# print 'Odom:', odom
pub.pub('vo', odom)
# time.sleep(0.1)
except:
# print 'Error:', e
raise
def __init__(self):
# Process.__init__(self)
super(tPub, self).__init__()
# self.daemon = True
self.pub = zmq.Pub(('127.0.0.1', 9000))
def __init__(self):
# Process.__init__(self)
# self.sub = zmq.Sub(['a', 'b'], ('127.0.0.1', 9000))
self.sub = zmq.Sub(['a'], ('127.0.0.1', 9000))
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 ...')