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 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 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 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 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 subscriber(**kwargs):
geckopy.init_node(**kwargs)
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
slam = RMHC_SLAM(LDS01_Model(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM')
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# callback function
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()
geckopy.Subscriber(['scan'], f)
geckopy.spin(20) # it defaults to 100hz, this is just to slow it down
print('sub bye ...')
def subscriber(**kwargs):
geckopy.init_node(**kwargs)
c = Callback()
geckopy.Subscriber(['camera'], c.callback)
geckopy.spin(2) # it defaults to 100hz, this is just to slow it down
print('sub 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 publisher(**kwargs):
geckopy.init_node()
exit = kwargs['exit']
pt = kwargs["pub"]
key = kwargs["key"]
topic = kwargs["topic"]
if pt == "bindtcp":
p = geckopy.pubBinderTCP(key, topic)
elif pt == "connecttcp":
p = geckopy.pubConnectTCP(key, topic)
elif pt == "binduds":
p = geckopy.pubBinderUDS(key, topic, "/tmp/pygecko_test")
elif pt == "connectuds":
p = geckopy.pubConnectUDS(key, topic)
if p is None:
assert False, "<<< Couldn't get Pub from geckocore >>>"
for _ in range(100):
if exit.is_set():
# print("exit")
break
p.publish(msg.SerializeToString())
time.sleep(0.1)
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 __init__(self, port, key):
geckopy.init_node()
self.pub = geckopy.pubBinderTCP(key, 'create')
self.sub = geckopy.subConnectTCP(key, 'cmd')
# Create a Create2
self.bot = Create2(port)
self.bot.start()
def go(**kwargs):
geckopy.init_node(**kwargs)
# global_logger = geckopy.log
test = kwargs.get('test', True)
robot = RobotTest(test)
# s = geckopy.Subscriber(['cmd'], robot.handle_msg)
geckopy.spin(5)
def subscriber(**kwargs):
geckopy.init_node(**kwargs)
topic = kwargs.get('topic')
c = Callback(topic)
geckopy.Subscriber([topic], c.callback)
# geckopy.on_shutdown(c.bye)
geckopy.spin(20) # it defaults to 100hz, this is just to slow it down
print('sub bye ...')
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 subscriber(**kwargs):
# geckopy = GeckoPy()
geckopy.init_node(**kwargs)
def f(topic, msg):
print(">> {}: {}".format(topic, msg))
topic = kwargs.get('topic')
geckopy.Subscriber([topic], f)
geckopy.spin()
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)
# p = geckopy.Publisher(topics=['test'])
# uds = kwargs.get('path')
p = Pub()
p.bind(kwargs.get('path'))
time.sleep(1) # need this!!
for msg in [msg1, msg2, msg3, msg4]:
# for msg in [msg1, msg2]:
p.publish(msg)
time.sleep(0.01)
def subscriber(**kwargs):
geckopy.init_node(**kwargs)
def callback(topic, msg):
img = msg2image(msg)
geckopy.logdebug('image timestamp: {:.3f}'.format(msg.timestamp))
cv2.imshow('image', img)
cv2.waitKey(10)
geckopy.Subscriber(['camera'], callback)
geckopy.spin(20) # it defaults to 100hz, this is just to slow it down
print('sub 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 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 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 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 subscribe2(**kwargs):
geckopy.init_node(**kwargs)
key = kwargs.get('key')
topic = kwargs.get('topic')
print(">>", kwargs)
print(">> Sub: {} {}".format(key, topic))
s = geckopy.subConnectTCP(key, topic)
while not geckopy.is_shutdown():
m = s.recv_nb()
# if 'img' in m:
# im = m['img']
# im = np.frombuffer(im, dtype=m['dtype'])
# im = im.reshape(m['shape'])
# geckopy.loginfo('image: {}x{}'.format(*im.shape[:2]))
# else:
if m:
geckopy.logwarn('msg: {}'.format(m))
chew_up_cpu(.2)
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 ...')
