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 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(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 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 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)
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 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 slam_publisher(**kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(10)
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))
# kludge until i fix the driver
scan = []
for i, p in enumerate(pts):
scan.append((
i,
p,
))
msg = Lidar(scan)
if test:
print("[LIDAR]=================")
print(" points: {}".format(len(msg.scan)))
print(" pt[0]: {}".format(msg.scan[0]))
print(" timestamp: {}".format(msg.timestamp))
obs = range_filter(scan, 200)
print("[Obstacles]=============")
print(" number: {}".format(len(obs)))
print(" obs: {}".format(obs))
else:
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 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)
