def main():
if True:
sp = GeckoSimpleProcess()
sp.start(func=core, name='geckocore', kwargs={})
print("<<< Starting Roomba >>>")
port = "/dev/serial/by-id/usb-FTDI_FT231X_USB_UART_DA01NX3Z-if00-port0"
bot = Create2(port)
bot.start()
bot.full()
geckopy.init_node()
rate = geckopy.Rate(10) # loop rate
# s = geckopy.subBinderUDS(key, 'cmd', "/tmp/cmd")
s = geckopy.subBinderTCP(key, 'cmd')
if s is None:
raise Exception("subscriber is None")
# p = geckopy.pubBinderUDS(key,'create2',"/tmp/create")
p = geckopy.pubBinderTCP(key, 'create2')
if p is None:
raise Exception("publisher is None")
print("<<< Starting Loop >>>")
try:
bot.drive_direct(200, 200)
while not geckopy.is_shutdown():
sensors = bot.get_sensors() # returns all data
batt = 100 * sensors.battery_charge / sensors.battery_capacity
# print(">> batter: {:.1f}".format(batt))
bot.digit_led_ascii("{:4}".format(int(batt)))
# bot.digit_led_ascii("80")
# print(">> ir:", sensors.light_bumper)
# print(">> ir:", end=" ")
# for i in range(6):
# print("{:.1f}".format(sensors[35 + i]), end=" ")
# print(" ")
# msg = sensors
# p.publish(msg)
msg = s.recv_nb()
if msg:
print(msg)
rate.sleep()
except KeyboardInterrupt:
print("bye ...")
bot.drive_stop()
# time.sleep(1)
# bot.close()
print("<<< Exiting >>>")
def msg_zmq(args):
# start message hub
# core = GeckoCore()
# core.start()
bs = BeaconCoreServer(key='test', handler=Ascii)
bs.start()
bs.run()
msg1 = imu_st(vec_t(1, 2, 3), vec_t(11, 12, 13), vec_t(21, 22, 23))
msg2 = twist_t(vec_t(1, 2, 3), vec_t(11, 12, 13))
msg3 = pose_t(vec_t(1, 2, 3), quaternion_t(1, 2, 3, 4))
msg4 = lidar_st(((1, 1), (2, 2), (3, 3)))
def publisher(**kwargs):
geckopy.init_node(**kwargs)
# p = geckopy.Publisher(topics=['test'])
# uds = kwargs.get('path')
p = Pub()
p.bind(kwargs.get('path'))
time.sleep(1) # need this!!
for msg in [msg1, msg2, msg3, msg4]:
# for msg in [msg1, msg2]:
p.publish(msg)
time.sleep(0.01)
p = GeckoSimpleProcess()
p.start(func=publisher, name='publisher', kwargs=args)
# subscriber
s = Sub()
s.topics = args.get('topics')
s.connect(args.get('path'))
for msg in [msg1, msg2, msg3, msg4]:
m = s.recv()
# assert t == b'test' # FIXME: fix stupid binary string crap!
assert msg == m
# core.join(0.1)
time.sleep(1) # if I run these too fast, I get errors on bind()
def test_pub_sub():
bs = BeaconCoreServer(key='test', handler=Ascii)
core = GeckoSimpleProcess()
core.start(func=bs.run, name='geckocore')
args = {
'key': 'test',
'topic': "test-tcp",
'pub': 'bindtcp',
'sub': 'connecttcp'
}
zmq_pub_sub(args)
args = {
'key': 'test',
'topic': "test-tcp-2",
'pub': 'connecttcp',
'sub': 'bindtcp'
}
zmq_pub_sub(args)
args = {
'key': 'test',
'topic': "test-uds",
'pub': 'binduds',
'sub': 'connectuds'
}
zmq_pub_sub(args)
args = {
'key': 'test',
'topic': "test-uds-2",
'pub': 'connectuds',
'sub': 'binduds'
}
zmq_pub_sub(args)
bs.stop()
core.join(0.1)
# core.start()
# although I don't do anything with procs, because I reuse the variables
# p and s below, they will kill the processes when new process are created
# using those names. Appending them to procs allows me to keep them alive
# until the program ends
procs = []
for topic in ['ryan', 'mike', 'sammie', 'scott']:
# threads_alive += 1
# info to pass to processes
args = {"key": "local", 'topic': topic}
p = GeckoSimpleProcess()
p.start(func=publisher, name='pub_{}'.format(topic), kwargs=args)
procs.append(p)
s = GeckoSimpleProcess()
s.start(func=subscriber, name='sub_{}'.format(topic), kwargs=args)
procs.append(s)
s = GeckoSimpleProcess()
s.start(func=subscriber, name='sub_{}_2'.format(topic), kwargs=args)
procs.append(s)
while True:
try:
time.sleep(1)
# print(">> Threads alive:", threads_alive)
# if threads_alive == 0:
# normally you wouldn't run this here, but is running else where
# this is just for testing
# from pygecko.transport import GeckoCore
# core = GeckoCore()
# core.start()
# although I don't do anything with procs, because I reuse the variables
# p and s below, they will kill the processes when new process are created
# using those names. Appending them to procs allows me to keep them alive
# until the program ends
procs = []
for topic in ['ryan', 'mike', 'sammie', 'scott']:
# info to pass to processes
args = {"key": "local", 'topic': topic}
p = GeckoSimpleProcess()
p.start(func=publisher, name='pub_{}'.format(topic), kwargs=args)
procs.append(p)
# s = GeckoSimpleProcess()
# s.start(func=subscriber, name='sub_{}'.format(topic), kwargs=args)
# procs.append(s)
while True:
try:
time.sleep(1)
except KeyboardInterrupt:
print('main process got ctrl-c')
break
if __name__ == '__main__':
# normally you wouldn't run this here, but is running else where
# this is just for testing
# from pygecko.transport import GeckoCore
# core = GeckoCore()
# core.start()
# although I don't do anything with procs, because I reuse the variables
# p and s below, they will kill the processes when new process are created
# using those names. Appending them to procs allows me to keep them alive
# until the program ends
procs = []
run = [imu_pub, camera_pub, matrix_sub]
for func in run:
p = GeckoSimpleProcess()
p.start(func=func)
procs.append(p)
# s = GeckoSimpleProcess()
# s.start(func=subscriber, name='sub_{}'.format(topic), kwargs=args)
# procs.append(s)
while True:
try:
time.sleep(1)
except KeyboardInterrupt:
print('main process got ctrl-c')
break
# from pygecko.transport import GeckoCore
# core = GeckoCore()
# core.start()
# although I don't do anything with procs, because I reuse the variables
# p and s below, they will kill the processes when new process are created
# using those names. Appending them to procs allows me to keep them alive
# until the program ends
procs = []
for topic in ['ryan', 'scott']:
# info to pass to processes
args = {'key': "local", 'topic': topic}
p = GeckoSimpleProcess()
p.start(func=publisher, name='pub_{}'.format(topic), kwargs=args)
procs.append(p)
args = {'key': "local"}
s = GeckoSimpleProcess()
c = Callback('class')
s.start(func=c.loop, name='sub', kwargs=args)
procs.append(s)
while True:
try:
time.sleep(1)
except KeyboardInterrupt:
print('main process got ctrl-c')
break
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)
# core = GeckoCore()
# core.start()
# although I don't do anything with procs, because I reuse the variables
# p and s below, they will kill the processes when new process are created
# using those names. Appending them to procs allows me to keep them alive
# until the program ends
procs = []
for topic in ['ryan', 'mike', 'sammie', 'scott']:
# info to pass to processes
args = {
"key": "local",
'topic': topic
}
# p = GeckoSimpleProcess()
# p.start(func=publisher, name='pub_{}'.format(topic), kwargs=args)
# procs.append(p)
s = GeckoSimpleProcess()
s.start(func=subscriber, name='sub_{}'.format(topic), kwargs=args)
procs.append(s)
while True:
try:
time.sleep(1)
except KeyboardInterrupt:
print('main process got ctrl-c')
break