# every few seconds.
#
if __name__ == '__main__':
if len(sys.argv) > 1:
image_directory = sys.argv[1]
if len(sys.argv) > 2:
start_index = int(sys.argv[2])
else:
first_index = 1000
if len(sys.argv) > 3:
seconds_per_plot = int(sys.argv[3])
lidar_viewer = LidarViewer()
file_index = start_index
while 1:
fname = "{:s}/lidar_snapshot_{:d}.dat".format(image_directory, file_index)
lidar_data = []
print("Loading file: {:s}\n".format(fname))
lidar_data = np.loadtxt(fname, delimiter=",")
# toss out the error data
error_free_data = []
if len(lidar_data) > 0:
error_free_data = np.array(filter(lambda x: x[1] < 777, lidar_data))
# plot any calculated markers in their specified color
#lidar_viewer.plot_markers(r2_markers, 'g')
lidar_viewer.plot_polar_markers(find_wall_markers, 'r')
#wall_markers = [max_start, max_end]
#lidar_viewer.plot_markers(wall_markers, 'b')
if __name__ == '__main__':
# put robot at the origin
robot = Robot((0,0), 0)
field_model = FieldModel()
lidar_viewer = LidarViewer()
factor = 4
while True:
text = raw_input("Enter command (mOVE,tURN,sHOW,iNFO,0(origin),hELP or qQUIT): ")
cmd = text.split(" ")[0]
#If we see a quit command, well, quit
if cmd == "q":
break
#Reset robot to the origin with 0 turn
if cmd == "0":
robot = Robot((0,0), 0)
elif cmd == "i":
x,y = robot.position
logging.basicConfig(format="%(asctime)s - %(name)s - %(levelname)s - %(message)s", level=logging.DEBUG)
logger = logging.getLogger(__name__)
#
# Open up the serial port, get lidar data and write it to a file
# every few seconds.
#
if __name__ == "__main__":
channel = UDPChannel(remote_ip="10.10.76.100", remote_port=5880, local_ip="0.0.0.0", local_port=52954)
# channel = UDPChannel()
range_at_heading_message = LidarRangeAtHeadingMessage()
periodic_message = LidarPeriodicMessage()
lidar_logger = LidarLogger(logger)
lidar_viewer = LidarViewer()
lp = None
# open up the serial port device connected to the lidar
try:
# lp = serial.Serial('/dev/ttyUSB0',115200,timeout=1)
lp = serial.Serial("/dev/tty.usbserial", 115200, timeout=1)
# lp = serial.Serial('/dev/tty.wchusbserial1420',115200,timeout=1)
except:
logger.error("Lidar port could not be opened.")
# connect the port with the laser and initialize the laser object
# lasr = OldLaser(lp)
lasr = Laser(lp)
def write_to_file(self, file_name):
LidarViewer.write_to_file(file_name, polar_data)
