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 = []
