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()
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 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()
def subscriber(self): msg = s.recv_nb() if msg: geckopy.loginfo("{}: {}".format(topic,msg))
def callback(self, topic, msg): geckopy.loginfo("{}".format(msg)) chew_up_cpu(.1)
def keypad_proc(**kwargs): """ This process handles the main keypad interface and sets the run state Also, MIGHT, do ultrasound and battery Keypad (https://www.adafruit.com/product/419) Pi pins (BCM) L connector R ----------------- 11 9 10 25 13 6 5 """ if platform.system() != 'Linux': gecko.logerror("{}: can only run on Linux".format(__FILE__)) geckopy.init_node(**kwargs) rate = geckopy.Rate(5) gecko.loginfo("Starting: keypad") kp = Keypad() current_state = 1 pub = geckopy.Publisher() while no geckopy.is_shutdown(): rate.sleep() # get keypad input key = kp.getKey() # if R2 has not fallen over, then check input if True: if key in [1, 2, 3]: if current_state != key: current_state = key pub.pub('state', current_state) elif key in [4, 5, 6]: # ns.emotion = key if key == 4: c = random.choice(["900", "help me", "religion", "moon", "smell"]) elif key == 5: # FIXME: make mp3 c = random.choice(["900", "help me", "religion", "moon", "smell"]) msg = Audio(c, None) pub.pub('audio', msg) elif key == 7: pub.pub('servo', Servo('wave')) elif key == 8: geckopy.loginfo("<<< got turn-off key press >>>") current_state = 0 break elif key == "#": # FIXME: not sure the right way to do this cleanly geckopy.loginfo("Shutting down") # shutdown = True # shutdown linux current_state = 0 break elif key == "*": geckopy.loginfo("Rebooting now") current_state = 0 # ns.reboot = True # reboot linux break # exiting current_state = 0 pub.pub('state', current_state) time.sleep(1)