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)
