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