forked from CharlieMonk/LIDAR-Brain
-
Notifications
You must be signed in to change notification settings - Fork 0
/
neeto_plotter.py
132 lines (116 loc) · 5.33 KB
/
neeto_plotter.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#!/usr/bin/env python
#laser.py
# Copyright 2012 Stephen Okay for Roadknight Labs
# Released under the GNU GPL V2
from __future__ import print_function
"""
XV11 utils
This program provides methods to get ranging data from a bare(i.e. not in a) Neato Robotics XV11 Laser Distance Scanner
running 2.6.x firmware
"""
import sys
import os
import serial
import pdb
import struct
import binascii
import collections
import itertools
import math
from time import sleep
import socket
from udp_channels import *
from sensor_message import *
from analyzer import Analyzer, aggregate_distance
from lidar_viewer import LidarLogger, LidarViewer
from laser import Laser, Reading, Packet, Rotation
import statistics
import logging
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)
#
#
slice_index = 0
file_index = 1
rotation_time = 0
current_time = 0
seconds_per_output = 1
SECONDS_PER_MINUTE = 60.0
rotations = []
while 1:
try:
rotation = Rotation(lasr.gather_full_rotation())
rotations = rotations[-4:]
rotations.append(rotation)
# rotation = OldRotation(lasr.gather_full_rotation())
#
# For now, we just output a lidar data snapshot every 10 seconds
# We will want this for debugging (maybe every second instead)
# change the "seconds_per_output" to tune that.
#
tgt_heading, tgt_range = Analyzer.range_at_heading(rotation.polar_data(), (Analyzer.start, Analyzer.stop))
logger.info("{:d} points yields {:.2f} inches at {:2d} degrees)".format(len(rotation.polar_data()),tgt_range, tgt_heading))
# push the newly calculated data into the message
range_at_heading_message.heading = tgt_heading
range_at_heading_message.range = tgt_range
channel.send_to(range_at_heading_message.encode_message())
# push periodic message to the bot
periodic_message.status = 'ok'
periodic_message.rpm = rotation.rpm()
channel.send_to(periodic_message.encode_message())
except IOError,e:
# log and notify robot of error
periodic_message.status = 'error'
channel.send_to(periodic_message.encode_message())
logger.error("Failed to gather a full rotation of data.")
#
# get revised instructions from robot
#
try:
robot_data, robot_address = channel.receive_from()
message_from_robot = RobotMessage(robot_data)
if ((message_from_robot.sender == 'robot') and
(message_from_robot.message == 'sweep')):
Analyzer.start = message_from_robot.start
Analyzer.stop = message_from_robot.stop
except socket.timeout:
logger.info("No message received from robot")
elapsed_time = (SECONDS_PER_MINUTE/float(rotation.rpm()))
rotation_time = rotation_time + elapsed_time
current_time = current_time + elapsed_time
if rotation_time > seconds_per_output:
sorted_rot = sorted(list(itertools.chain(*rotations)), key = lambda(theta,_): theta)
grouped_by = itertools.groupby(sorted_rot, lambda x: x[0])
groups = [(key, list(items)) for key, items in grouped_by]
polar_data = [(angle, aggregate_distance(ranges)) for angle, ranges in groups]
lidar_viewer.plot_polar(polar_data)
lidar_viewer.plot_cartesian(rotation.cartesian_data())
lidar_logger.log_data(rotation.polar_data())
pdb.set_trace()
file_index = file_index + 1
rotation_time = 0
rotations = []