def __init__(self, port, key):
geckopy.init_node()
self.pub = geckopy.pubBinderTCP(key, 'create')
self.sub = geckopy.subConnectTCP(key, 'cmd')
# Create a Create2
self.bot = Create2(port)
self.bot.start()
def sub(**kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(2)
s = geckopy.subConnectTCP("local", "bob2")
if (s == None):
print("ERROR setting up subscriber")
return
cnt = 0
while not geckopy.is_shutdown():
data = s.recv_nb()
print("sub:", data)
print('-' * 40)
rate.sleep()
def loop(self, **kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(2)
sr = geckopy.subConnectTCP(kwargs.get('key'), 'ryan')
ss = geckopy.subConnectTCP(kwargs.get('key'), 'scott')
p = geckopy.pubBinderTCP(kwargs.get('key'), 'ans')
start = time.time()
while not geckopy.is_shutdown():
m = sr.recv_nb()
if m:
self.r = m
m = ss.recv_nb()
if m:
self.s = m
msg = {'ans': self.s + self.r}
p.publish(msg) # topic msg
geckopy.logdebug('ans msg: {}'.format(msg))
rate.sleep()
print('pub bye ...')
def subscriber(**kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(2)
logger = geckopy.getLogger(__name__)
s = geckopy.subConnectTCP(kwargs.get('key'), kwargs.get('topic'))
if s is None:
logger.error("subscriber is None")
return
while not geckopy.is_shutdown():
ss = s.recv_nb()
if ss:
msg = protobufUnpack(ss, Vector)
logger.info("sub: {}".format(msg))
chew_up_cpu(.1)
rate.sleep()
print('sub bye ...')
def subscribe2(**kwargs):
geckopy.init_node(**kwargs)
key = kwargs.get('key')
topic = kwargs.get('topic')
print(">>", kwargs)
print(">> Sub: {} {}".format(key, topic))
s = geckopy.subConnectTCP(key, topic)
while not geckopy.is_shutdown():
m = s.recv_nb()
# if 'img' in m:
# im = m['img']
# im = np.frombuffer(im, dtype=m['dtype'])
# im = im.reshape(m['shape'])
# geckopy.loginfo('image: {}x{}'.format(*im.shape[:2]))
# else:
if m:
geckopy.logwarn('msg: {}'.format(m))
chew_up_cpu(.2)
def subscriber(**kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(2)
topic = kwargs.get('topic')
# c = Callback(topic)
s = geckopy.subConnectTCP(kwargs.get('key'), kwargs.get('topic'))
if s is None:
print("subscriber is None")
# global threads_alive
# threads_alive -= 1
return
while not geckopy.is_shutdown():
msg = s.recv_nb()
if msg:
geckopy.loginfo("{}: {}".format(topic, msg))
chew_up_cpu(.1)
rate.sleep()
print('sub bye ...')
def zmq_pub_sub(args):
geckopy.init_node()
# stop pub
exit = Event()
exit.clear()
msg = Imu()
msg.linear_acceleration.x = 1
msg.linear_acceleration.y = 11
msg.linear_acceleration.z = 111
msg.angular_velocity.x = 2
msg.angular_velocity.y = 22.22
msg.angular_velocity.z = 222.22
msg.magnetic_field.x = 3
msg.magnetic_field.y = 33
msg.magnetic_field.z = 333
msg.orientation.w = 1
msg.orientation.x = 0
msg.orientation.y = 0
msg.orientation.z = 0
msg.timestamp = time.time()
def publisher(**kwargs):
geckopy.init_node()
exit = kwargs['exit']
pt = kwargs["pub"]
key = kwargs["key"]
topic = kwargs["topic"]
if pt == "bindtcp":
p = geckopy.pubBinderTCP(key, topic)
elif pt == "connecttcp":
p = geckopy.pubConnectTCP(key, topic)
elif pt == "binduds":
p = geckopy.pubBinderUDS(key, topic, "/tmp/pygecko_test")
elif pt == "connectuds":
p = geckopy.pubConnectUDS(key, topic)
if p is None:
assert False, "<<< Couldn't get Pub from geckocore >>>"
for _ in range(100):
if exit.is_set():
# print("exit")
break
p.publish(msg.SerializeToString())
time.sleep(0.1)
p = GeckoSimpleProcess()
args['exit'] = exit
p.start(func=publisher, name='publisher', kwargs=args)
st = args["sub"]
key = args["key"]
topic = args["topic"]
if st == "connecttcp":
s = geckopy.subConnectTCP(key, topic)
elif st == "bindtcp":
s = geckopy.subBinderTCP(key, topic)
elif st == "connectuds":
s = geckopy.subConnectUDS(key, topic)
elif st == "binduds":
s = geckopy.subBinderUDS(key, topic, "/tmp/pygecko_test_2")
for _ in range(5):
mm = s.recv()
if mm:
exit.set()
break
else:
print(".", end=" ", flush=True)
time.sleep(0.1)
m = Imu()
m.ParseFromString(mm)
assert msg == m, "{} => {}".format(msg, m)
#!/usr/bin/env python3
from pygecko.multiprocessing import geckopy
from pygecko.messages import lidar_st
from the_collector import BagIt
from the_collector import Pickle
if __name__ == '__main__':
bag = BagIt(Pickle)
geckopy.init_node()
rate = geckopy.Rate(10)
s = geckopy.subConnectTCP('dalek', 'lidar')
if s is None:
raise Exception("subscriber is None")
# try:
while not geckopy.is_shutdown():
msg = s.recv_nb()
if msg:
# geckopy.loginfo("{}".format(msg))
print('.', end='', flush=True)
bag.push('lidar', msg)
rate.sleep()
# except KeyboardInterrupt:
bag.write('lidar')
print('sub bye ...')
def zmq_pub_sub(args):
geckopy.init_node()
# stop pub
exit = Event()
exit.clear()
msg = imu_st(vec_t(1, 2, 3), vec_t(11, 12, 13), vec_t(21, 22, 23))
def publisher(**kwargs):
geckopy.init_node()
exit = kwargs['exit']
pt = kwargs["pub"]
key = kwargs["key"]
topic = kwargs["topic"]
if pt == "bindtcp":
p = geckopy.pubBinderTCP(key, topic)
elif pt == "connecttcp":
p = geckopy.pubConnectTCP(key, topic)
elif pt == "binduds":
p = geckopy.pubBinderUDS(key, topic, "/tmp/pygecko_test")
elif pt == "connectuds":
p = geckopy.pubConnectUDS(key, topic)
if p is None:
assert False, "<<< Couldn't get Pub from geckocore >>>"
for _ in range(100):
if exit.is_set():
# print("exit")
break
p.publish(msg)
time.sleep(0.1)
p = GeckoSimpleProcess()
args['exit'] = exit
p.start(func=publisher, name='publisher', kwargs=args)
st = args["sub"]
key = args["key"]
topic = args["topic"]
if st == "connecttcp":
s = geckopy.subConnectTCP(key, topic)
elif st == "bindtcp":
s = geckopy.subBinderTCP(key, topic)
elif st == "connectuds":
s = geckopy.subConnectUDS(key, topic)
elif st == "binduds":
s = geckopy.subBinderUDS(key, topic, "/tmp/pygecko_test_2")
for _ in range(5):
m = s.recv()
if m:
exit.set()
break
else:
print(".", end=" ", flush=True)
time.sleep(0.1)
assert msg == m, "{} => {}".format(msg, m)