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)