def pcv(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(10) p = geckopy.Publisher() camera = WebcamVideoStream(src=0).start() while not geckopy.is_shutdown(): img = camera.read() if img is not None: # geckopy.log(img.shape) # img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) img = cv2.resize(img, (640,480)) msg = find_ball(img) # msg = Image(img.shape, img.tobytes(), img.dtype) if msg: # p.pub('images_color', msg) # topic msg p.pub('target', msg) geckopy.log(msg) # p.pub(topic, {'a': 5}) # geckopy.log(img.shape) else: geckopy.log("*** couldn't read image ***") # sleep rate.sleep() camera.stop() print('cv bye ...')
def publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(10) # 10 Hz # p = geckopy.Publisher(['camera']) p = geckopy.pubBinderTCP(kwargs.get('key'), kwargs.get('topic')) if p is None: raise Exception("publisher is None") # determine if we should use picamera or standard usb camera # if platform.system() == 'Linux': # picam = True # else: # picam = False # # camera = VideoStream(usePiCamera=picam) # camera.start() while not geckopy.is_shutdown(): img = camera.read() # img = cv2.resize(img, (320, 240)) # img = cv2.resize(img, (640, 480)) img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) msg = image2msg(img) p.publish('camera', msg) rate.sleep() print('pub bye ...')
def imu_publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(10) p = geckopy.Publisher() if platform.system() == 'Linux': isLinux = True imu = IMU_hw() else: isLinux = False imu = None while not geckopy.is_shutdown(): if isLinux: a, m, g = imu.get() # geckopy.log('{:.1f} {:.1f} {:.1f}'.format(*a)) msg = IMU(Vector(*a), Vector(*m), Vector(*g)) else: # fake readings msg = IMU( Vector(1, 2, 3), Vector(1, 2, 3), Vector(1, 2, 3), ) p.pub('imu', msg) # msg = Vector(0,0,1) # p.pub('unit_accel', msg) # sleep rate.sleep()
def movement(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(1) path = [ (1.0,0,0,), (1.0,0,0,), (1.0,0,0,), (1.0,0,0,), (1.0,0,0,), (1.0,0,0,), ] p = geckopy.Publisher() i = 0 cmd = itertools.cycle(path) while not geckopy.is_shutdown(): msg = next(cmd) p.pub('cmd', msg) # topic msg geckopy.log(msg) # geckopy.log('[{}] published: {}'.format(i, msg)) # i = (i + 1) % len(path) rate.sleep() print('pub bye ...')
def publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(10) pub = geckopy.Publisher() if platform.system() == 'Linux': args = { 'src': 0, 'usePiCamera': True, 'resolution': ( 640, 480, ), 'framerate': 10 } else: args = {'src': 0, 'usePiCamera': False} cam = VideoStream(**args).start() time.sleep(1) # camera check img = cam.read() print(img.shape) cv2.imwrite('test.png', img) while not geckopy.is_shutdown(): img = cam.read() # img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) msg = Image(img.shape, img.tobytes()) pub.pub('camera', msg) rate.sleep() cam.stop()
def subscriber(**kwargs): geckopy.init_node(**kwargs) MAP_SIZE_PIXELS = 500 MAP_SIZE_METERS = 10 slam = RMHC_SLAM(LDS01_Model(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM') mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) # callback function def f(topic, msg): # print("recv[{}]: {}".format(topic, msg)) geckopy.loginfo(msg.timestamp) pts = msg.scan slam.update(pts) # Get current robot position x, y, theta = slam.getpos() # Get current map bytes as grayscale slam.getmap(mapbytes) display.displayMap(mapbytes) display.setPose(x, y, theta) display.refresh() geckopy.Subscriber(['scan'], f) geckopy.spin(20) # it defaults to 100hz, this is just to slow it down print('sub bye ...')
def subscriber(**kwargs): geckopy.init_node(**kwargs) c = Callback() geckopy.Subscriber(['camera'], c.callback) geckopy.spin(2) # it defaults to 100hz, this is just to slow it down print('sub bye ...')
def imu_publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(10) p = geckopy.Publisher() test = kwargs.get('test', False) imu = IMU_hw() while not geckopy.is_shutdown(): if test: # fake readings msg = IMU( Vector(1, 2, 3), Vector(1, 2, 3), Vector(1, 2, 3), ) else: a, m, g = imu.get() geckopy.log('{:.1f} {:.1f} {:.1f}'.format(*a)) msg = IMU(Vector(*a), Vector(*m), Vector(*g)) p.pub('imu', msg) msg = Vector(0, 0, 1) p.pub('unit_accel', msg) # sleep rate.sleep()
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)
def lidar_publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(1) geckopy.log(kwargs) p = geckopy.Publisher() test = kwargs.get('test', False) # MAP_SIZE_PIXELS = 500 # MAP_SIZE_METERS = 10 if platform.system() == 'Linux': port = '/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0' else: port = "/dev/tty.SLAB_USBtoUART" # Connect to Lidar unit lidar = LDS01() lidar.open(port) lidar.run(True) # Create an RMHC SLAM object with a laser model and optional robot model # slam = RMHC_SLAM(LDS01_Model(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display # display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') # Initialize empty map # mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) while not geckopy.is_shutdown(): # Update SLAM with current Lidar scan pts = lidar.read() # need to reverse the order for it to plot correctly pts = list(reversed(pts)) msg = Lidar(pts) p.pub('scan', msg) # slam.update(pts) # Get current robot position # x, y, theta = slam.getpos() # Get current map bytes as grayscale # slam.getmap(mapbytes) # display.displayMap(mapbytes) # display.setPose(x, y, theta) # display.refresh() # p.pub('avoid', msg) # geckopy.log(msg) # sleep rate.sleep() # all done lidar.close()
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 go(**kwargs): geckopy.init_node(**kwargs) # global_logger = geckopy.log test = kwargs.get('test', True) robot = RobotTest(test) # s = geckopy.Subscriber(['cmd'], robot.handle_msg) geckopy.spin(5)
def subscriber(**kwargs): geckopy.init_node(**kwargs) topic = kwargs.get('topic') c = Callback(topic) geckopy.Subscriber([topic], c.callback) # geckopy.on_shutdown(c.bye) geckopy.spin(20) # it defaults to 100hz, this is just to slow it down print('sub bye ...')
def publisher(**kwargs): geckopy.init_node() rate = geckopy.Rate(10) tcp = kwargs["useTcp"] key = kwargs["key"] if tcp: p = pubBinderTCP(key, 'twist_kb') else: p = pubBinderUDS(key, 'twist_kb', fname=kwargs["udsfile"]) if p is None: return ang = [0, 0, 0] lin = [0, 0, 0] while not geckopy.is_shutdown(): # 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) if key not in ['a', 'd', 'w', 'x', 's', 'q']: continue print('>>>', key) if key == 'a': ang[2] += 0.1 ang[2] = limit_max(ang[2]) elif key == 'd': ang[2] -= 0.1 ang[2] = limit_min(ang[2]) elif key == 'w': lin[0] += 0.1 lin[0] = limit_max(lin[0]) elif key == 'x': lin[0] -= 0.1 lin[0] = limit_min(lin[0]) elif key == 's': # stop - all 0's ang = [0, 0, 0] lin = [0, 0, 0] elif key == 'q': break twist = twist_t(vec_t(*lin), vec_t(*ang)) p.publish(twist) # topic msg rate.sleep()
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 subscriber(**kwargs): # geckopy = GeckoPy() geckopy.init_node(**kwargs) def f(topic, msg): print(">> {}: {}".format(topic, msg)) topic = kwargs.get('topic') geckopy.Subscriber([topic], f) geckopy.spin()
def publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(1) p = geckopy.Publisher(['data']) while not geckopy.is_shutdown(): msg = {'a': 1} p.pub('data', msg) rate.sleep() print('pub bye ...')
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)
def subscriber(**kwargs): geckopy.init_node(**kwargs) def callback(topic, msg): img = msg2image(msg) geckopy.logdebug('image timestamp: {:.3f}'.format(msg.timestamp)) cv2.imshow('image', img) cv2.waitKey(10) geckopy.Subscriber(['camera'], callback) geckopy.spin(20) # it defaults to 100hz, this is just to slow it down print('sub bye ...')
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 publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(2) topic = kwargs.get('topic') p = geckopy.pubBinderTCP(kwargs.get('key'), topic) start = time.time() cnt = 0 while not geckopy.is_shutdown(): msg = cnt p.publish(msg) # topic msg geckopy.logdebug('{}[{}] published msg'.format(topic, cnt)) cnt += 1 rate.sleep() print('pub bye ...')
def imu_pub(): geckopy.init_node() rate = geckopy.Rate(2) p = geckopy.pubBinderTCP(KEY, "imu") if p is None: raise Exception("publisher is None") imu = NXP_IMU() while not geckopy.is_shutdown(): msg = imu.get() p.publish(msg) # topic msg rate.sleep() print('imu pub bye ...')
def publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(2) topic = kwargs.get('topic') p = geckopy.Publisher(topic) start = time.time() cnt = 0 while not geckopy.is_shutdown(): msg = {'time': time.time() - start} p.pub(topic, msg) # topic msg geckopy.logdebug('[{}] published msg'.format(cnt)) cnt += 1 rate.sleep() print('pub bye ...')
def pub(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(2) p = geckopy.pubBinderTCP("local", "bob2") if (p == None): print("ERROR setting up publisher") return cnt = 0 v = vec_t(1, 2, 3) m = imu_st(v, v, v) while not geckopy.is_shutdown(): # m = imu_st(v,v,v) p.publish(m) print("sent") rate.sleep() cnt += 1
def camera_pub(): geckopy.init_node() rate = geckopy.Rate(2) p = geckopy.pubBinderTCP(KEY, "camera") if p is None: raise Exception("publisher is None") cam = PiCamera() while not geckopy.is_shutdown(): # img = cam.read() img = True if img: msg = "hi" p.publish(msg) rate.sleep() print('camera pub bye ...')
def publisher(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(2) p = geckopy.pubBinderTCP(kwargs.get('key'), kwargs.get('topic')) if p is None: print("** publisher is None") return start = time.time() cnt = 0 while not geckopy.is_shutdown(): msg = {'time': time.time() - start} p.publish(msg) # topic msg geckopy.logdebug('[{}] published msg'.format(cnt)) cnt += 1 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 publish(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(1) key = kwargs.get('key') topic = kwargs.get('topic') p = geckopy.pubBinderTCP(key, topic) datumn = time.time() while not geckopy.is_shutdown(): msg = { 'time': time.time() - datumn, 'double': 3.14, 'int': 5, 'array': [1, 2, 3, 4, 5] } p.publish(msg) rate.sleep() print('pub 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 ...')