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 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 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 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 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 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