from collections import deque import numpy as np from rplidar import RPLidar import pygame # from occupancy_grid import OccupancyGrid serial_port = '/dev/ttyUSB0' lidar = RPLidar(port=serial_port) print("Sent RESET command...") lidar.reset() time.sleep(1) model, fw, hw, serial_no = lidar.get_device_info() health_status, err_code = lidar.get_device_health() print(''' === Opened LIDAR on serial port {} Model ID: {} Firmware: {} Hardware: {} Serial Number: {} Device Health Status: {} (Error Code: 0x{:X}) === '''.format(serial_port, model, fw, hw, serial_no.hex(), health_status, err_code)) pygame.init()
# Add some labels plt.xlabel("X-Location (meters)") plt.ylabel("Y-Location (meters)") ax.add_patch(radius_view) ax.add_patch(roomba) if x_y is not None: ax.plot(x_y[0], x_y[1], 'o') if __name__ == '__main__': lidar = RPLidar('/dev/ttyUSB0') lidar.connect() print lidar.get_health() print lidar.get_device_info() lidar.start_scan() time.sleep(2) # Let that baby warm up set_plot() plt.ion() plt.show() try: while True: point_list = [p for p in lidar._raw_points] rp_point_list = [ RPLidarPoint(raw_point) for raw_point in point_list ] rp_point_list.sort(key=lambda v: v.angle)
elif args.front == 3: ax.plot([-1 * (y) for y in x_y[1]], [-1 * (x) for x in x_y[0]], "-" if args.plot_lines else "o") elif args.front == 4: ax.plot(x_y[0], [-1 * (y) for y in x_y[1]], "-" if args.plot_lines else "o") # else: # ax.plot(x_y[0], x_y[1], 'o') # This is just for debugging what is wrong with the above TODO if __name__ == "__main__": parse_args() print args.front, type(args.front) lidar = RPLidar(args.port_name) lidar.connect() print lidar.get_health() print lidar.get_device_info() lidar.start_scan() time.sleep(2) # Let that baby warm up set_plot() plt.ion() plt.show() try: while True: point_list = [p for p in lidar._raw_points] rp_point_list = [RPLidarPoint(raw_point) for raw_point in point_list] rp_point_list.sort(key=lambda v: v.angle) _x = [] _y = []