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 = 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']: # 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)
# 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
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