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 ...')
