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 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 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 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 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()
'--topic',
help='log topic name, default is "log"',
default='log')
args = vars(parser.parse_args())
return args
def format_print(topic, msg):
# print(msg.level)
# msg format: {proc_name, level, text}
if msg.level == 'DEBUG': color = Fore.CYAN
elif msg.level == 'WARN': color = Fore.YELLOW
elif msg.level == 'ERROR': color = Fore.RED
else: color = Fore.GREEN
# shorten proc names??
print(Style.BRIGHT + color + '>> {}:'.format(msg.name[:8]) +
Style.RESET_ALL + msg.text)
# print(">> {}: {}".format(topic, msg))
if __name__ == "__main__":
args = handleArgs()
topic = args['topic']
kwargs = {'host': args['host']}
geckopy.init_node(**kwargs)
geckopy.Subscriber([topic], format_print)
geckopy.spin()