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 imu_publisher(**kwargs): # geckopy.init_node(**kwargs) rate = geckopy.Rate(20) # p = geckopy.Publisher() # test = kwargs.get('test', False) imu = IMU_hw() save = [] # while not geckopy.is_shutdown(): for i in range(400): a, m, g = imu.get() # geckopy.log('{:.1f} {:.1f} {:.1f}'.format(*a)) print(">> {}".format(i % 20)) msg = IMU(Vector(*a), Vector(*m), Vector(*g)) save.append(msg) # p.pub('imu', msg) # msg = Vector(0,0,1) # p.pub('unit_accel', msg) # sleep rate.sleep() pickle.dump(save, open("accel-z-up.pickle", "wb"))
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(**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 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 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 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 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 test_rate(): rate = geckopy.Rate(10) start = time.time() for _ in range(10): rate.sleep() stop = time.time() # print(stop - start) assert (stop - start) + 0.05 > 1.0
def loop(self): if self.pub is None: raise Exception("publisher is None") if self.sub is None: raise Exception("subscriber is None") rate = geckopy.Rate(10) while not geckoopy.is_shutdown(): self.publisher() self.subscriber() rate.sleep()
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 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 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 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 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 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 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 publisher(**kwargs): geckopy.init_node(**kwargs) topic = kwargs.get('topic') msg = kwargs.get('msg') hertz = kwargs.get('rate', 10) p = geckopy.Publisher([topic]) rate = geckopy.Rate(hertz) cnt = 0 start = time.time() while not geckopy.is_shutdown(): p.pub(topic, msg) # topic msg if cnt % hertz == 0: print(">> {}[{:.1f}]: published {} msgs".format( topic, time.time() - start, hertz)) cnt += 1 rate.sleep()
def publisher(**kwargs): geckopy.init_node() rate = geckopy.Rate(2) logger = geckopy.getLogger(__name__) p = geckopy.pubBinderTCP(kwargs.get('key'), kwargs.get('topic')) if p is None: logger.error("publisher is None") return cnt = 0 while geckopy.ok(): msg = Vector() msg.x = 1 msg.y = 2 msg.z = 3 p.publish(protobufPack(msg)) # topic msg logger.debug('[{}] published msg'.format(cnt)) cnt += 1 rate.sleep() print('pub bye ...')
def matrix_sub(): geckopy.init_node() rate = geckopy.Rate(10) # s = geckopy.subConnectTCP( # KEY, # "test" # ) # # if s is None: # raise Exception("subscriber is None") m = MatrixArray([0x70, 0x71, 0x72, 0x73], brightness=0) while not geckopy.is_shutdown(): # msg = s.recv_nb() # if msg: # geckopy.loginfo("{}: {}".format(msg)) m.random() rate.sleep() print('sub bye ...')
def matrix(**kwargs): geckopy.init_node(**kwargs) rate = geckopy.Rate(1) test = kwargs.get('test', False) matrix = LEDDisplay() s = geckopy.Subscriber(['led'], got_msg) i = 0 while not geckopy.is_shutdown(): # if s has message, call function # else update led geckopy.log('x') # matrix.setRandom() matrix.update() # matrix.set(1,1,127) # matrix.clear() # matrix.display.set_pixel(i//8,i%8, 1) # matrix.write() # i = (i+1)%64 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 i2c_proc(**kwargs): """ Everything attached to i2c bus goes here so we don't have to do semifores. Also, the lcd button code is here too so it is always in sync with the led matricies. """ if platform.system() != 'Linux': gecko.logerror("{}: can only run on Linux".format(__FILE__)) return geckopy.init_node(kwargs) rate = geckopy.Rate(10) geckopy.loginfo("Starting i2c process") # pub = geckopy.Publisher() # imu = IMU(gs=4, dps=2000, verbose=False) # leds = LogicFunctionDisplay(led_data) # led_update = 0 # # servos = [] # servo_angles = [] # for id in servo_limits: # s = Servo(id) # # s.setMinMax(*servo_limits[id]) # set open(min)/closed(max) angles # s.open = servo_limits[id][0] # s.close = servo_limits[id][1] # s.closeDoor() # closed # # servo_angles.append(sum(servo_limits[id])/2) # servos.append(s) # servo_angles.append(s.angle) # time.sleep(0.01) # Servo.all_stop() # b_led = ButtonLED(26,16,20) # test --------------------- # vals = [True]*3 # b_led.setRGB(*vals) # time.sleep(3) # for i in range(3): # print(i) # vals = [True]*3 # vals[i] = False # b_led.setRGB(*vals) # time.sleep(3) # while flag.is_set(): # a, m, g = imu.get() # pub.pub(IMU(a,m,g)) # FIXME: real hw is at a funny oriendataion # a = normalize(a) # seems that 0.80 is pretty big tilt # if a[2] < 0.85: # ns.safety_kill = True # print(a) # print('<<< TILT >>>') # update LEDs # OFF = 0 # GREEN = 1 # RED = 2 # YELLOW = 3 led_update += 1 if led_update % 20 == 0: led_update = 0 # print('current_state',ns.current_state) cs, batt = ns.current_state, ns.battery if cs == 1: # standby csc = 2 # red b_led.setRGB(True, False, False) elif cs == 2: # static csc = 3 # yellow b_led.setRGB(True, True, True) elif cs == 3: # remote csc = 1 # green b_led.setRGB(False, True, False) # make something up for now # battc = random.randint(1,3) if ns.set_all_leds: leds.setAll(ns.set_all_leds) else: leds.setFLD(csc, 1) leds.setRLD() # update servos if the have changed # namespace.servo_angles: another process wants to change the angle # servo_angles: local copy, if no difference between the 2, do nothing # TODO: should these just be open/close (T/F)? why angles? for nsa, sa, servo in zip(ns.servo_angles, servo_angles, servos): if nsa == sa: continue sa = nsa servo.angle = sa time.sleep(0.1) if ns.servo_wave: ns.servo_wave = False leds.setAll(1) print('servo wave') for s in servos: s.openDoor() time.sleep(0.2) # servos[1].stop() time.sleep(2) # Servo.all_stop() for s in servos: s.closeDoor() time.sleep(0.2) # servos[1].stop() time.sleep(2) Servo.all_stop()
#!/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 ...')