def log_plotter_init(self):
if self.first_run:
if self.fname.split('.')[-1] == 'csv':
self.file = ReadCsvFile(
self.fname,
sonarPort=4002,
posPort=13102,
cont_reading=self.cont_reading_checkbox.checkState())
else:
self.file = ReadLogFile(self.fname)
self.grid = RawGrid(self.ogrid_conditions[0],
self.ogrid_conditions[1],
self.ogrid_conditions[2],
self.ogrid_conditions[3])
self.raw_grid = RawPlot(self.raw_res, self.ogrid_conditions[1],
self.ogrid_conditions[2])
self.img_item.scale(self.grid.cellSize, self.grid.cellSize)
self.img_item.setPos(-self.grid.XLimMeters, -self.grid.YLimMeters)
self.timer.timeout.connect(self.log_updater)
self.first_run = False
if self.pause:
self.timer.start(0)
self.pause = False
if self.start_rec_on_moos_start:
self.start_rec()
self.start_rec_on_moos_start = False
self.start_plotting_button.setText('Stop plotting ')
else:
self.stop_plot()
def run_morse(self):
if self.first_run:
self.grid = RawGrid(self.ogrid_conditions[0],
self.ogrid_conditions[1],
self.ogrid_conditions[2],
self.ogrid_conditions[3],
self.ogrid_conditions[4])
self.raw_grid = RawPlot(self.raw_res, self.ogrid_conditions[1],
self.ogrid_conditions[2])
self.img_item.scale(self.grid.cellSize, self.grid.cellSize)
self.img_item.setPos(-self.grid.XLimMeters, -self.grid.YLimMeters)
self.first_run = False
else:
self.grid.clear_grid()
self.raw_grid.clearGrid()
if self.morse_running:
self.moos_client.close()
self.morse_running = False
self.stop_plot()
self.from_morse_button.setText('From Morse')
else:
self.moos_client = MoosMsgs()
self.moos_client.set_on_sonar_msg_callback(
self.moos_sonar_message_recieved)
self.moos_client.set_on_pos_msg_callback(
self.moos_pos_message_recieved)
self.moos_client.run()
self.morse_running = True
self.timer.timeout.connect(self.moos_updater)
self.timer.start(0)
self.pause = False
self.from_morse_button.setText('Stop from morse ')
if self.start_rec_on_moos_start:
self.start_rec()
self.start_rec_on_moos_start = False
def __init__(self):
self.moos = MoosMsgs()
self.moos.set_on_sonar_msg_callback(self.print_msg)
# moos.set_on_pos_msg_callback(print_pos_msg)
self.moos.run()
self.ogrid_conditions = [0.1, 20, 15, 0.5]
self.grid = RawGrid(self.ogrid_conditions[0], self.ogrid_conditions[1], self.ogrid_conditions[2],
self.ogrid_conditions[3])
def init_grid(self):
if Settings.update_type == 1:
self.grid = OccupancyGrid(GridSettings.half_grid,
GridSettings.p_inital,
GridSettings.p_occ, GridSettings.p_free,
GridSettings.p_binary_threshold,
GridSettings.cell_factor)
elif Settings.update_type == 0:
self.grid = RawGrid(GridSettings.half_grid)
class Test(object):
def __init__(self):
self.moos = MoosMsgs()
self.moos.set_on_sonar_msg_callback(self.print_msg)
# moos.set_on_pos_msg_callback(print_pos_msg)
self.moos.run()
self.ogrid_conditions = [0.1, 20, 15, 0.5]
self.grid = RawGrid(self.ogrid_conditions[0], self.ogrid_conditions[1], self.ogrid_conditions[2],
self.ogrid_conditions[3])
def print_msg(self, msg):
a = self.grid.get_p()
print('heading: {}\tlength: {}\n{}'.format(msg.bearing, msg.length, msg.data))
return True
class MainWidget(QtGui.QWidget):
plot_updated = False
grid = None
contour_list = []
collision_stat = 0
thread_pool = QtCore.QThreadPool()
def __init__(self, parent=None):
super(MainWidget, self).__init__(parent)
# if Settings.input_source == 0:
# raise NotImplemented
# self.last_pos_msg = None
# elif Settings.input_source == 1:
# self.last_pos_msg = MoosPosMsg()
# self.last_pos_diff = MoosPosMsgDiff(0, 0, 0)
self.last_pos_msg = MoosPosMsg()
self.last_pos_diff = MoosPosMsgDiff(0, 0, 0)
main_layout = QtGui.QHBoxLayout() # Main layout
left_layout = QtGui.QVBoxLayout()
right_layout = QtGui.QVBoxLayout()
graphics_view = pg.GraphicsLayoutWidget(
) # layout for holding graphics object
self.plot_window = pg.PlotItem()
graphics_view.addItem(self.plot_window)
# IMAGE Window
self.img_item = pg.ImageItem(autoLevels=False)
if Settings.plot_type != 2:
colormap = pg.ColorMap(PlotSettings.steps_raw,
np.array(PlotSettings.colors))
self.img_item.setLookupTable(colormap.getLookupTable(mode='byte'))
self.plot_window.addItem(self.img_item)
self.plot_window.getAxis('left').setGrid(200)
self.img_item.getViewBox().invertY(True)
self.img_item.getViewBox().setAspectLocked(True)
self.img_item.setOpts(axisOrder='row-major')
# Textbox
self.threshold_box = QtGui.QSpinBox()
self.threshold_box.setMinimum(0)
self.threshold_box.setMaximum(20000)
self.threshold_box.setValue(GridSettings.threshold)
# Randomize button
self.randomize_button = QtGui.QPushButton('Randomize')
self.randomize_button.clicked.connect(self.randomize_occ_grid)
# wp straight ahead button
self.wp_straight_ahead_button = QtGui.QPushButton('Wp Straight Ahed')
self.wp_straight_ahead_button.clicked.connect(
self.wp_straight_ahead_clicked)
# Collision margin box
self.collision_margin_box = QtGui.QDoubleSpinBox()
self.collision_margin_box.setMinimum(0)
self.collision_margin_box.setValue(CollisionSettings.obstacle_margin)
self.collision_margin_box.setSingleStep(0.5)
self.collision_margin_box.valueChanged.connect(
self.update_collision_margin)
# binary plot
self.binary_plot_button = QtGui.QPushButton('Send Initial WPs')
self.binary_plot_button.clicked.connect(self.binary_button_click)
# Clear grid button
self.clear_grid_button = QtGui.QPushButton('Clear Grid!')
self.clear_grid_button.clicked.connect(self.clear_grid)
# Adding items
self.threshold_box.setMaximumSize(Settings.button_width,
Settings.button_height)
self.collision_margin_box.setMaximumSize(Settings.button_width,
Settings.button_height)
self.binary_plot_button.setMaximumSize(Settings.button_width,
Settings.button_height)
self.clear_grid_button.setMaximumSize(Settings.button_width,
Settings.button_height)
self.randomize_button.setMaximumSize(Settings.button_width,
Settings.button_height)
self.wp_straight_ahead_button.setMaximumSize(Settings.button_width,
Settings.button_height)
left_layout.addWidget(self.threshold_box)
left_layout.addWidget(self.collision_margin_box)
left_layout.addWidget(self.binary_plot_button)
left_layout.addWidget(self.clear_grid_button)
left_layout.addWidget(self.randomize_button)
left_layout.addWidget(self.wp_straight_ahead_button)
# left_layout.setGeometry(QtCore.QRect(0, 0, 200, 10**6))
# left_layout.SizeHint(QtCore.QRect(0, 0,))
# print(left_layout.maximumSize(200, 0))
right_layout.addWidget(graphics_view)
main_layout.addLayout(left_layout)
main_layout.addLayout(right_layout)
if Settings.collision_avoidance and Settings.show_map:
self.map_widget = map.MapWidget()
self.map_widget.setMaximumSize(800, 10**6)
main_layout.addWidget(self.map_widget)
self.setLayout(main_layout)
if Settings.show_pos:
pos_layout = QtGui.QVBoxLayout()
text_layout = QtGui.QHBoxLayout()
value_layout = QtGui.QHBoxLayout()
N_text = QtGui.QLabel('North')
N_text.setLineWidth(3)
E_text = QtGui.QLabel('East')
E_text.setLineWidth(3)
H_text = QtGui.QLabel('Heading')
H_text.setLineWidth(3)
self.north = QtGui.QLabel('0')
self.north.setLineWidth(3)
self.east = QtGui.QLabel('0')
self.east.setLineWidth(3)
self.heading = QtGui.QLabel('0')
self.heading.setLineWidth(3)
text_layout.addWidget(N_text)
text_layout.addWidget(E_text)
text_layout.addWidget(H_text)
value_layout.addWidget(self.north)
value_layout.addWidget(self.east)
value_layout.addWidget(self.heading)
pos_layout.addLayout(text_layout)
pos_layout.addLayout(value_layout)
right_layout.addLayout(pos_layout)
if LosSettings.enable_los:
cross_track_text = QtGui.QLabel('Cross-track error')
cross_track_text.setLineWidth(3)
along_track_text = QtGui.QLabel('Along-track error')
along_track_text.setLineWidth(3)
self.cross_track = QtGui.QLabel('0')
self.cross_track.setLineWidth(3)
self.along_track = QtGui.QLabel('0')
self.along_track.setLineWidth(3)
text_layout.addWidget(cross_track_text)
text_layout.addWidget(along_track_text)
value_layout.addWidget(self.cross_track)
value_layout.addWidget(self.along_track)
if Settings.save_scan_lines:
self.scan_lines = []
if Settings.hist_window:
self.hist_window = pg.PlotWindow()
self.histogram = pg.BarGraphItem(x=np.arange(10),
y1=np.random.rand(10),
width=0.3,
brush='r')
self.hist_window.addItem(self.histogram)
self.hist_window.show()
if Settings.show_voronoi_plot:
self.voronoi_window = pg.PlotWindow()
self.voronoi_plot_item = pg.ImageItem(autoLevels=False)
self.voronoi_window.addItem(self.voronoi_plot_item)
self.voronoi_plot_item.getViewBox().invertY(True)
self.voronoi_plot_item.getViewBox().setAspectLocked(True)
self.voronoi_plot_item.setOpts(axisOrder='row-major')
self.voronoi_window.show()
self.init_grid()
if Settings.input_source == 0:
self.udp_client = UdpClient(
ConnectionSettings.sonar_port, ConnectionSettings.pos_port,
ConnectionSettings.autopilot_ip,
ConnectionSettings.autopilot_server_port,
ConnectionSettings.autopilot_listen_port)
self.udp_client.signal_new_sonar_msg.connect(self.new_sonar_msg)
self.udp_client.set_sonar_callback(self.new_sonar_msg)
self.udp_client.start()
# self.udp_thread = threading.Thread(target=self.udp_client.start)
# self.udp_thread.setDaemon(True)
# self.udp_thread.start()
elif Settings.input_source == 1:
self.moos_msg_client = MoosMsgs(self.new_sonar_msg)
self.moos_msg_client.signal_new_sonar_msg.connect(
self.new_sonar_msg)
self.moos_msg_client.run()
else:
raise Exception('Uknown input source')
self.last_pos_msg = None
self.processing_pos_msg = False
self.pos_lock = threading.Lock()
self.timer = QtCore.QTimer()
self.timer.timeout.connect(self.update_plot)
self.pos_update_timer = QtCore.QTimer()
self.pos_update_timer.timeout.connect(self.new_pos_msg)
if Settings.plot_type == 2:
self.timer.start(500.0)
else:
self.timer.start(1000.0 / 24.0)
if Settings.collision_avoidance:
if Settings.input_source == 1:
if Settings.show_voronoi_plot:
self.collision_avoidance = CollisionAvoidance(
self.moos_msg_client, self.voronoi_plot_item)
else:
self.collision_avoidance = CollisionAvoidance(
self.moos_msg_client)
else:
if Settings.show_voronoi_plot:
self.collision_avoidance = CollisionAvoidance(
self.udp_client, self.voronoi_plot_item)
else:
self.collision_avoidance = CollisionAvoidance(
self.udp_client)
self.collision_worker = CollisionAvoidanceWorker(
self.collision_avoidance)
self.collision_worker.setAutoDelete(False)
self.collision_worker.signals.finished.connect(
self.collision_loop_finished)
self.collision_avoidance_timer = QtCore.QTimer()
self.collision_avoidance_timer.setSingleShot(True)
self.collision_avoidance_timer.timeout.connect(
self.collision_avoidance_loop)
self.collision_avoidance_timer.start(
Settings.collision_avoidance_interval)
self.grid_worker = GridWorker(self.grid)
self.grid_worker.setAutoDelete(False)
self.grid_worker.signals.finished.connect(self.grid_worker_finished)
self.pos_update_timer.start(Settings.pos_update_speed)
self.grid_worker_finished(True)
def init_grid(self):
if Settings.update_type == 1:
self.grid = OccupancyGrid(GridSettings.half_grid,
GridSettings.p_inital,
GridSettings.p_occ, GridSettings.p_free,
GridSettings.p_binary_threshold,
GridSettings.cell_factor)
elif Settings.update_type == 0:
self.grid = RawGrid(GridSettings.half_grid)
def clear_grid(self):
# self.grid.clear_grid()
# self.plot_updated = False
# self.update_plot()
self.grid_worker.clear_grid()
@QtCore.pyqtSlot(object, name='new_sonar_msg')
def new_sonar_msg(self, msg):
self.grid_worker.add_data_msg(msg, self.threshold_box.value())
self.plot_updated = True
if Settings.save_scan_lines:
self.scan_lines.append(msg.data)
if len(self.scan_lines) > 100:
np.savez('pySonarLog/scan_lines_{}'.format(
strftime("%Y%m%d-%H%M%S")),
scan_lines=np.array(self.scan_lines))
self.scan_lines = []
def new_pos_msg(self):
with self.pos_lock:
if Settings.input_source == 0:
msg = self.udp_client.cur_pos_msg
if msg is None:
return
else:
msg = self.moos_msg_client.cur_pos_msg
if Settings.show_pos:
self.north.setText('{:.2f}'.format(msg.north))
self.east.setText('{:.2f}'.format(msg.east))
self.heading.setText('{:.1f}'.format(msg.yaw * 180.0 / np.pi))
if LosSettings.enable_los and Settings.enable_autopilot:
if Settings.input_source == 0:
e, s = self.udp_client.los_controller.get_errors()
self.collision_avoidance.update_external_wps(
wp_counter=self.udp_client.los_controller.
get_wp_counter())
else:
e, s = self.moos_msg_client.los_controller.get_errors()
if Settings.collision_avoidance:
self.collision_avoidance.update_external_wps(
wp_counter=self.moos_msg_client.los_controller.
get_wp_counter())
self.along_track.setText('{:.2f}'.format(s))
self.cross_track.setText('{:.2f}'.format(e))
# if self.last_pos_msg is None:
# self.last_pos_msg = deepcopy(msg)
if Settings.collision_avoidance:
self.collision_avoidance.update_pos(msg)
if Settings.show_map:
self.map_widget.update_pos(msg.north, msg.east, msg.yaw,
self.grid.range_scale)
# self.map_widget.update_avoidance_waypoints(self.collision_avoidance.new_wp_list)
self.grid_worker.update(msg)
self.last_pos_msg = deepcopy(msg)
# diff = (msg - self.last_pos_msg)
# trans = self.grid.trans(diff.dx, diff.dy)
# rot = self.grid.rot(diff.dyaw)
# if trans or rot:
# self.plot_updated = True
# if self.thread_pool.activeThreadCount() < self.thread_pool.maxThreadCount():
# diff = (msg - self.last_pos_msg)
# self.last_pos_msg = deepcopy(msg)
#
# self.thread_pool.start(self.grid_worker, 6)
# logger.debug('Start grid worker: {} of {}'.format(self.thread_pool.activeThreadCount(), self.thread_pool.maxThreadCount()))
# else:
# logger.debug('Skipped iteration because of few available threads')
def update_plot(self):
if self.plot_updated:
if Settings.plot_type == 0:
self.img_item.setImage(self.grid.get_raw(),
levels=(0.0, 255.0),
autoLevels=False)
elif Settings.plot_type == 1:
if self.binary_plot_on:
self.img_item.setImage(self.grid.get_binary_map(),
levels=(0, 1),
autoLevels=False)
else:
self.img_item.setImage(self.grid.get_p(),
levels=(0.0, 1.0),
autoLevels=False)
elif Settings.plot_type == 2:
if Settings.update_type == 1:
self.img_item.setImage(np.zeros((1601, 1601, 3)))
image, contours = self.grid.get_obstacles()
self.img_item.setImage(image)
a = 1
else:
im, contours = self.grid.adaptive_threshold(
self.threshold_box.value())
if Settings.collision_avoidance:
self.collision_avoidance.update_obstacles(
contours, self.grid.range_scale)
if Settings.show_wp_on_grid:
if self.collision_stat is CollisionStatus.NO_DANGER or self.collision_stat is CollisionStatus.NEW_ROUTE_OK:
ok = True
else:
ok = False
if self.last_pos_msg is None:
im = self.collision_avoidance.draw_wps_on_grid(
image, (0, 0, 0), ok)
else:
im = self.collision_avoidance.draw_wps_on_grid(
image, (self.last_pos_msg.north,
self.last_pos_msg.east,
self.last_pos_msg.yaw), ok)
if Settings.show_map:
self.map_widget.update_obstacles(contours,
self.grid.range_scale,
self.last_pos_msg.north,
self.last_pos_msg.east,
self.last_pos_msg.yaw)
self.img_item.setImage(im)
else:
raise Exception('Invalid plot type')
if self.grid.last_distance is not None:
self.img_item.setPos(
-self.grid.last_distance, -self.grid.last_distance /
2 if GridSettings.half_grid else -self.grid.last_distance)
# self.img_item.scale(self.grid.last_distance/self.grid.RES, self.grid.last_distance/self.grid.RES)
if Settings.hist_window:
hist, _ = np.histogram(self.grid.get_raw(), 256)
print(np.sum(hist[1:] * np.arange(1, 256) / np.sum(hist[1:])))
self.histogram.setOpts(x=np.arange(255),
y1=hist[1:],
height=1000)
self.hist_window.setYRange(max=1000, min=0)
# if Settings.collision_avoidance and Settings.show_map:
# self.map_widget.repaint()
self.plot_updated = False
def collision_avoidance_loop(self):
self.collision_worker.set_reliable(self.grid.reliable)
self.thread_pool.start(self.collision_worker, 6)
# logger.debug('Start collision worker: {} of {}'.format(self.thread_pool.activeThreadCount(), self.thread_pool.maxThreadCount()))
@QtCore.pyqtSlot(CollisionStatus, name='collision_worker_finished')
def collision_loop_finished(self, status):
self.collision_stat = status
if status is CollisionStatus.NO_FEASIBLE_ROUTE or status is CollisionStatus.SMOOTH_PATH_VIOLATES_MARGIN:
left = np.mean(self.grid.grid[:, :800])
right = np.mean(self.grid.grid[:, 801:])
if CollisionSettings.send_new_wps:
if Settings.input_source == 0:
self.udp_client.stop_and_turn(
np.sign(left - right) * 15 * np.pi / 180.0)
else:
self.moos_msg_client.stop_and_turn(
np.sign(left - right) * 15 * np.pi / 180.0)
if Settings.show_map:
self.map_widget.invalidate_wps()
self.collision_avoidance_timer.start(0)
return
elif status is CollisionStatus.NEW_ROUTE_OK:
if Settings.show_wp_on_grid:
self.plot_updated = True
if Settings.show_map:
self.map_widget.update_avoidance_waypoints(
self.collision_avoidance.new_wp_list)
# if Settings.save_obstacles and status is not CollisionStatus.NO_DANGER:
# self.grid_worker.save_obs()
self.collision_avoidance_timer.start(
Settings.collision_avoidance_interval)
@QtCore.pyqtSlot(bool, name='grid_worker_finished')
def grid_worker_finished(self, status):
if status:
self.plot_updated = True
self.thread_pool.start(self.grid_worker, 6)
# logger.debug('Start grid worker: {} of {}'.format(self.thread_pool.activeThreadCount(), self.thread_pool.maxThreadCount()))
@QtCore.pyqtSlot(object, name='new_wp')
def wp_received(self, var):
if type(var) is list:
self.collision_avoidance.update_external_wps(var, None)
elif type(var) is int:
self.collision_avoidance.update_external_wps(None, var)
else:
raise Exception('Unknown object type in wp_received slot')
if Settings.show_map:
wp_list, wp_counter = self.collision_avoidance.data_storage.get_wps(
)
self.map_widget.update_waypoints(wp_list, wp_counter,
self.collision_stat)
def binary_button_click(self):
if Settings.collision_avoidance:
# # Round course
# wp_list = [[5.5667, -25.4974, 3],
# [8.5557, -15.7151, 3],
# [13.1751, -3.7589, 3],
# [7.7405, 7.382, 3],
# [-0.41141, 18.523, 3],
# [-14.2697, 16.3492, 3],
# [-21.6064, -0.76989, 3],
# [-17.5305, -13.5412, 3]]
# col course
wp_list = [[-10.4082, 11.0137, 3], [-3.9653, 1.7359, 3],
[3.2508, 1.9936, 3], [12.0131, 0.44728, 3],
[14.3326, -12.954, 3], [24.3835, -24.2935, 3]]
if self.last_pos_msg is None:
self.last_pos_msg = MoosPosMsg(wp_list[0][0], wp_list[0][1],
np.pi)
self.collision_avoidance.update_pos(self.last_pos_msg)
wp0 = vehicle2NED(0.5, 0, self.last_pos_msg.north,
self.last_pos_msg.east, self.last_pos_msg.yaw)
wp_list.insert(0, [wp0[0], wp0[1], 2.0])
wp_list = fermat(wp_list)[0]
self.collision_avoidance.update_external_wps(wp_list, 0)
self.collision_avoidance.save_paths(wp_list)
if Settings.input_source == 0:
self.udp_client.update_wps(wp_list)
else:
self.moos_msg_client.update_wps(wp_list)
if Settings.show_map:
self.map_widget.update_waypoints(wp_list, 0)
self.plot_updated = True
def wp_straight_ahead_clicked(self):
if Settings.collision_avoidance == True:
if self.grid.range_scale == 1:
self.grid.range_scale = 30
with self.pos_lock:
if self.last_pos_msg is None:
self.last_pos_msg = MoosPosMsg(6821592.4229, 457959.8588,
0, 2)
self.collision_avoidance.update_pos(self.last_pos_msg)
wp1 = vehicle2NED(
self.grid.range_scale * .05 *
CollisionSettings.dummy_wp_factor[0],
self.grid.range_scale * .05 *
CollisionSettings.dummy_wp_factor[1],
self.last_pos_msg.north, self.last_pos_msg.east,
self.last_pos_msg.yaw)
wp2 = vehicle2NED(
self.grid.range_scale * .2 *
CollisionSettings.dummy_wp_factor[0],
self.grid.range_scale * .2 *
CollisionSettings.dummy_wp_factor[1],
self.last_pos_msg.north, self.last_pos_msg.east,
self.last_pos_msg.yaw)
wp3 = vehicle2NED(
self.grid.range_scale *
CollisionSettings.dummy_wp_factor[0],
self.grid.range_scale *
CollisionSettings.dummy_wp_factor[1],
self.last_pos_msg.north, self.last_pos_msg.east,
self.last_pos_msg.yaw)
wp0 = vehicle2NED(1, 0, self.last_pos_msg.north,
self.last_pos_msg.east,
self.last_pos_msg.yaw)
wp0 = [wp0[0], wp0[1], 140, 0.5]
wp1 = [wp1[0], wp1[1], 140, 0.5]
wp2 = [wp2[0], wp2[1], 140, 0.5]
wp3 = [wp3[0], wp3[1], 140, 0.5]
self.collision_avoidance.update_external_wps([wp0, wp1, wp2, wp3],
0)
if Settings.input_source == 0:
self.udp_client.update_wps([wp0, wp1])
else:
self.moos_msg_client.update_wps([wp0, wp1, wp2, wp3])
if Settings.show_map:
self.map_widget.update_waypoints([wp0, wp1, wp2, wp3], 0)
self.plot_updated = True
def randomize_occ_grid(self):
if Settings.update_type == 1:
# self.grid.randomize()
# self.plot_updated = True
self.grid_worker.randomize()
# if Settings.collision_avoidance == True:
# # self.collision_avoidance.update_obstacles(self.grid.get_obstacles()[1], self.grid.range_scale)
# self.wp_straight_ahead_clicked()
# else:
self.plot_updated = True
def update_collision_margin(self):
CollisionSettings.obstacle_margin = self.collision_margin_box.value()
class MainWidget(QtGui.QWidget):
binary_plot = True
#ogrid_conditions = [0.1, 16, 8, 0.65, 0.78]
ogrid_conditions = [0.3, 60, 30, 0.65, 0.78]
raw_res = 0.05
old_pos_msg = 0
grid = 0
morse_running = False
latest_sonar_msg_moose = None
latest_pos_msg_moose = None
old_pos_msg_moose = None
counter = 0
counter2 = 0
cur_lat = 0
cur_long = 0
cur_head = 0
first_run = True
pause = True
start_rec_on_moos_start = False
# fname = 'logs/360 degree scan harbour piles.V4LOG' #inital file name
fname = 'logs/UdpHubLog_4001_2017_11_02_09_01_58.csv'
# fname = '/home/orjangr/Repos/pySonar/logs/Sonar Log/UdpHubLog_4001_2017_11_02_09_16_58.csv'
def __init__(self, parent=None):
super(MainWidget, self).__init__(parent)
main_layout = QtGui.QHBoxLayout() # Main layout
left_layout = QtGui.QVBoxLayout()
right_layout = QtGui.QGridLayout()
bottom_right_layout = QtGui.QGridLayout()
graphics_view = pg.GraphicsLayoutWidget(
) # layout for holding graphics object
# view_box = pg.ViewBox(invertY=True) # making viewbox for the image, inverting y to make it right
self.plot_window = pg.PlotItem()
graphics_view.addItem(self.plot_window)
# IMAGE Window
self.img_item = pg.ImageItem(autoLevels=False,
levels=(0,
1)) # image item. the actual plot
colormap = pg.ColorMap([0, 0.33, 0.67, 1],
np.array([[0.2, 0.2, 0.2, 0],
[0.0, 1.0, 1.0, 1.0],
[1.0, 1.0, 0.0, 1.0],
[1.0, 0.0, 0.0, 1.0]]))
self.img_item.setLookupTable(colormap.getLookupTable(mode='byte'))
self.plot_window.addItem(self.img_item)
self.plot_window.getAxis('left').setGrid(200)
self.img_item.getViewBox().invertY(True)
# raw plot
raw_graphics_view = pg.GraphicsLayoutWidget(
) # layout for holding graphics object
# view_box = pg.ViewBox(invertY=True) # making viewbox for the image, inverting y to make it right
self.raw_plot_window = pg.PlotItem()
raw_graphics_view.addItem(self.raw_plot_window)
self.raw_img_item = pg.ImageItem(
autoLevels=False, levels=(0, 255)) # image item. the actual plot
self.raw_img_item.setLookupTable(colormap.getLookupTable())
self.raw_plot_window.addItem(self.raw_img_item)
self.raw_plot_window.getAxis('left').setGrid(200)
self.raw_img_item.getViewBox().invertY(True)
# Scanline plot
scan_line_widget = pg.PlotWidget()
self.scan_line_plot = scan_line_widget.getPlotItem()
self.scan_line = self.scan_line_plot.plot(np.array([0, 0]),
np.array([0, 0]))
self.scan_line_plot.setYRange(0, 255)
# Button
self.start_plotting_button = QtGui.QPushButton('Start Plotting')
# Textbox
self.threshold_box = QtGui.QSpinBox()
self.threshold_box.setMinimum(0)
self.threshold_box.setMaximum(255)
self.threshold_box.setValue(45)
# Select file
self.select_file_button = QtGui.QPushButton('Select File')
# From Morse
self.from_morse_button = QtGui.QPushButton('From Morse')
# Check box for continious reading
self.cont_reading_checkbox = QtGui.QCheckBox()
self.cont_reading_checkbox.setText('Read multiple files')
self.cont_reading_checkbox.setChecked(True)
# binary plot
self.binary_plot_button = QtGui.QPushButton('Set Prob mode')
# Clear grid
self.clear_grid_button = QtGui.QPushButton('Clear Grid!')
self.start_rec_button = QtGui.QPushButton('Start Rec')
# Time box
self.msg_date = QtGui.QLineEdit()
self.msg_date.setReadOnly(True)
self.msg_time = QtGui.QLineEdit()
self.msg_time.setReadOnly(True)
self.date_text = QtGui.QLineEdit()
self.date_text.setReadOnly(True)
#Lat long rot boxes
self.lat_box = QtGui.QLineEdit()
self.lat_box.setReadOnly(True)
self.long_box = QtGui.QLineEdit()
self.long_box.setReadOnly(True)
self.rot_box = QtGui.QLineEdit()
self.rot_box.setReadOnly(True)
# Adding items
left_layout.addWidget(self.threshold_box)
left_layout.addWidget(self.start_plotting_button)
left_layout.addWidget(self.select_file_button)
left_layout.addWidget(self.cont_reading_checkbox)
left_layout.addWidget(self.from_morse_button)
left_layout.addWidget(self.binary_plot_button)
left_layout.addWidget(self.clear_grid_button)
left_layout.addWidget(self.start_rec_button)
# view_box.addItem(self.img_item)
# graphics_view.addItem(view_box)
bottom_right_layout.addWidget(self.msg_date, 0, 0, 1, 1)
bottom_right_layout.addWidget(self.msg_time, 0, 1, 1, 1)
bottom_right_layout.addWidget(self.lat_box, 1, 0, 1, 1)
bottom_right_layout.addWidget(self.long_box, 1, 1, 1, 1)
bottom_right_layout.addWidget(self.rot_box, 1, 2, 1, 1)
right_layout.addWidget(graphics_view, 0, 0, 1, 1)
right_layout.addWidget(scan_line_widget, 0, 1, 2, 1)
right_layout.addWidget(raw_graphics_view, 1, 0, 1, 1)
right_layout.addLayout(bottom_right_layout, 3, 0, 1, 1)
main_layout.addLayout(left_layout)
main_layout.addLayout(right_layout)
self.setLayout(main_layout)
#register button presses
self.start_plotting_button.clicked.connect(self.log_plotter_init)
self.select_file_button.clicked.connect(self.getFile)
self.from_morse_button.clicked.connect(self.run_morse)
self.clear_grid_button.clicked.connect(self.clear_img)
self.binary_plot_button.clicked.connect(self.binary_plot_clicked)
self.start_rec_button.clicked.connect(self.start_rec)
#######
self.timer = QtCore.QTimer()
self.timer_save = QtCore.QTimer()
self.timer_save_log = QtCore.QTimer()
# self.colormap = plt.get_cmap('OrRd')
self.rec_started = False
self.timer_save.timeout.connect(self.moos_save_img)
self.timer_save_log.timeout.connect(self.log_save_img)
def run_morse(self):
if self.first_run:
self.grid = RawGrid(self.ogrid_conditions[0],
self.ogrid_conditions[1],
self.ogrid_conditions[2],
self.ogrid_conditions[3],
self.ogrid_conditions[4])
self.raw_grid = RawPlot(self.raw_res, self.ogrid_conditions[1],
self.ogrid_conditions[2])
self.img_item.scale(self.grid.cellSize, self.grid.cellSize)
self.img_item.setPos(-self.grid.XLimMeters, -self.grid.YLimMeters)
self.first_run = False
else:
self.grid.clear_grid()
self.raw_grid.clearGrid()
if self.morse_running:
self.moos_client.close()
self.morse_running = False
self.stop_plot()
self.from_morse_button.setText('From Morse')
else:
self.moos_client = MoosMsgs()
self.moos_client.set_on_sonar_msg_callback(
self.moos_sonar_message_recieved)
self.moos_client.set_on_pos_msg_callback(
self.moos_pos_message_recieved)
self.moos_client.run()
self.morse_running = True
self.timer.timeout.connect(self.moos_updater)
self.timer.start(0)
self.pause = False
self.from_morse_button.setText('Stop from morse ')
if self.start_rec_on_moos_start:
self.start_rec()
self.start_rec_on_moos_start = False
def start_rec(self):
if not (self.morse_running or not self.pause):
self.start_rec_on_moos_start = True
return 1
if self.rec_started:
self.timer_save.stop()
self.start_rec_button.setText('Start Rec')
self.rec_started = False
if self.morse_running:
self.timer_save.stop()
else:
self.timer_save_log.stop()
else:
self.counter_save_scanline = 0
self.lastest_head_diff = 0
self.last_msg = MoosSonarMsg()
self.last_msg.bearing = 0
self.latest_head_diff = 0
self.new_sonar_msg = None
if self.morse_running:
self.timer_save.start(0)
else:
self.timer_save_log.start(0)
self.start_rec_button.setText('Stop Rec')
self.rec_started = True
def moos_sonar_message_recieved(self, msg):
logger.info('bearing : {}'.format(msg.bearing * 180 / pi))
self.latest_sonar_msg_moose = msg
self.new_sonar_msg = msg
return True
def moos_pos_message_recieved(self, msg):
self.latest_pos_msg_moose = msg
return True
def moos_updater(self):
updated = False
# self.moos_client.send_speed(0.1, 0.001)
if self.latest_sonar_msg_moose:
self.grid.auto_update_zhou(self.latest_sonar_msg_moose,
self.threshold_box.value())
#self.raw_grid.autoUpdate(self.latest_sonar_msg_moose)
#self.raw_img_item.setImage(self.raw_grid.grid.T)
#self.scan_line.setData(np.arange(len(self.latest_sonar_msg_moose.data)), self.latest_sonar_msg_moose.data)
updated = True
self.latest_sonar_msg_moose = None
if self.latest_pos_msg_moose:
if not self.old_pos_msg_moose:
self.old_pos_msg_moose = self.latest_pos_msg_moose
self.cur_lat = self.latest_pos_msg_moose.lat
self.cur_long = self.latest_pos_msg_moose.long
self.cur_head = self.latest_pos_msg_moose.head
delta_msg = self.latest_pos_msg_moose - self.old_pos_msg_moose
if delta_msg.x != 0 or delta_msg.y != 0 or delta_msg.head != 0:
self.grid.translational_motion(
delta_msg.y, delta_msg.x) # ogrid reference frame
self.grid.rotate_grid(delta_msg.head)
self.lat_box.setText('Lat: {:G}'.format(
self.latest_pos_msg_moose.lat))
self.long_box.setText('Long: {:G}'.format(
self.latest_pos_msg_moose.long))
self.rot_box.setText('Heading: {:G} deg'.format(
self.latest_pos_msg_moose.head * 180 / pi))
updated = True
self.old_pos_msg_moose = self.latest_pos_msg_moose
self.latest_pos_msg_moose = None
if updated:
if not self.binary_plot:
self.counter += 1
if self.counter == 5:
self.img_item.setImage(self.grid.get_p().T)
self.counter = 0
else:
self.img_item.setImage(self.grid.get_binary_map().T)
def moos_save_img(self):
if self.new_sonar_msg:
if not (np.sign(self.latest_head_diff)
== np.sign(self.new_sonar_msg.bearing -
self.last_msg.bearing)):
self.counter2 += 1
scipy.io.savemat('sonar_plots/logs/bin_grid_{}.mat'.format(
self.counter2),
mdict={
'bin_grid': self.grid.get_binary_map(),
'lat': self.cur_lat,
'long': self.cur_long,
'head': self.cur_head,
'xmax': self.grid.XLimMeters,
'ymax': self.grid.YLimMeters,
'time': self.new_sonar_msg.time
})
scipy.io.savemat('sonar_plots/logs/prob_grid_{}.mat'.format(
self.counter2),
mdict={
'prob': self.grid.get_p(),
'lat': self.cur_lat,
'long': self.cur_long,
'head': self.cur_head,
'xmax': self.grid.XLimMeters,
'ymax': self.grid.YLimMeters
})
# scipy.io.savemat('sonar_plots/logs/scanline_{}.mat'.format(self.counter2),
# mdict={'scanline': self.data_msgs, 'lat': self.lat_msgs,
# 'long': self.long_msgs, 'head': self.head_msgs, 'bearing': self.bearing_msgs})
scipy.io.savemat('sonar_plots/logs/raw_grid_{}.mat'.format(
self.counter2),
mdict={
'raw_grid': self.raw_grid.grid,
'xmax': self.grid.XLimMeters,
'ymax': self.grid.YLimMeters
})
self.latest_head_diff = self.new_sonar_msg.bearing - self.last_msg.bearing
self.last_msg = self.new_sonar_msg
self.new_sonar_msg = None
def log_save_img(self):
if self.new_sonar_msg:
if not (np.sign(self.latest_head_diff)
== np.sign(self.new_sonar_msg.bearing -
self.last_msg.bearing)):
self.counter2 += 1
scipy.io.savemat('sonar_plots/logs/bin_grid_{}.mat'.format(
self.counter2),
mdict={
'bin_grid': self.grid.get_binary_map(),
'xmax': self.grid.XLimMeters,
'ymax': self.grid.YLimMeters
})
scipy.io.savemat('sonar_plots/logs/prob_grid_{}.mat'.format(
self.counter2),
mdict={
'prob': self.grid.get_p(),
'xmax': self.grid.XLimMeters,
'ymax': self.grid.YLimMeters
})
# scipy.io.savemat('sonar_plots/logs/scanline_{}.mat'.format(self.counter2),
# mdict={'scanline': self.data_msgs, 'nbins': self.nbins})
scipy.io.savemat('sonar_plots/logs/raw_grid_{}.mat'.format(
self.counter2),
mdict={
'raw_grid': self.raw_grid.grid,
'xmax': self.grid.XLimMeters,
'ymax': self.grid.YLimMeters
})
self.latest_head_diff = self.new_sonar_msg.bearing - self.last_msg.bearing
self.last_msg = self.new_sonar_msg
self.new_sonar_msg = None
def log_plotter_init(self):
if self.first_run:
if self.fname.split('.')[-1] == 'csv':
self.file = ReadCsvFile(
self.fname,
sonarPort=4002,
posPort=13102,
cont_reading=self.cont_reading_checkbox.checkState())
else:
self.file = ReadLogFile(self.fname)
self.grid = RawGrid(self.ogrid_conditions[0],
self.ogrid_conditions[1],
self.ogrid_conditions[2],
self.ogrid_conditions[3])
self.raw_grid = RawPlot(self.raw_res, self.ogrid_conditions[1],
self.ogrid_conditions[2])
self.img_item.scale(self.grid.cellSize, self.grid.cellSize)
self.img_item.setPos(-self.grid.XLimMeters, -self.grid.YLimMeters)
self.timer.timeout.connect(self.log_updater)
self.first_run = False
if self.pause:
self.timer.start(0)
self.pause = False
if self.start_rec_on_moos_start:
self.start_rec()
self.start_rec_on_moos_start = False
self.start_plotting_button.setText('Stop plotting ')
else:
self.stop_plot()
def log_updater(self):
msg = self.file.read_next_msg()
updated = False
if msg == -1:
self.stop_plot()
elif msg != 0:
if msg.sensor == 2:
while msg.type != 2:
msg = self.file.read_next_msg()
self.grid.auto_update_zhou(msg, self.threshold_box.value())
self.raw_grid.autoUpdate(msg)
self.raw_img_item.setImage(self.raw_grid.grid.T)
updated = True
self.scan_line.setData(np.arange(len(msg.data)), msg.data)
self.new_sonar_msg = msg
elif msg.sensor == 1:
if not self.old_pos_msg:
self.old_pos_msg = msg
delta_msg = msg - self.old_pos_msg
if delta_msg.x != 0 or delta_msg.y != 0 or delta_msg.head != 0:
self.grid.translational_motion(
delta_msg.y, delta_msg.x) # ogrid reference frame
self.grid.rotate_grid(delta_msg.head)
self.lat_box.setText('Lat: {:G}'.format(msg.lat))
self.long_box.setText('Long: {:G}'.format(msg.long))
self.rot_box.setText('Heading: {:G} deg'.format(msg.head))
logger.debug(
'Delta x: {}\tDeltaY: {}\tDelta psi: {} deg'.format(
delta_msg.x, delta_msg.y,
delta_msg.head * pi / 180))
self.msg_date.setText('Date: {}'.format(msg.date))
self.msg_time.setText('Time: {}'.format(msg.time))
if updated:
if not self.binary_plot:
self.img_item.setImage(self.grid.get_p().T)
else:
self.img_item.setImage(self.grid.get_binary_map().T)
def clear_img(self):
self.img_item.setImage(np.zeros(np.shape(self.img_item.image)))
if self.grid:
self.grid.clear_grid()
self.raw_grid.clearGrid()
def getFile(self):
self.stop_plot()
self.fname = QtGui.QFileDialog.getOpenFileName(
self.parent(), 'Open log file', 'logs/',
"Log files (*.csv *.V4LOG)")
self.first_run = True
if self.grid:
self.grid.clear_grid()
self.clear_img()
def stop_plot(self):
self.timer.stop()
self.timer_save.stop()
self.pause = True
self.start_plotting_button.setText('Start plotting')
def binary_plot_clicked(self):
self.binary_plot = not self.binary_plot
if self.binary_plot:
self.binary_plot_button.setText('Set Prob mode')
else:
self.binary_plot_button.setText('Set Binary mode')
from math import pi
import numpy as np
from ogrid.rawGrid import RawGrid
O = RawGrid(0.1, 20, 15, 0.5)
print(len(O.bearing_ref))
step = np.array([4, 8, 16, 32]) * O.GRAD2RAD
step = step[2]
theta = np.linspace(-pi / 2, pi / 2, pi / step)
a = 1
for i in range(0, len(theta)):
O.updateCells(O.sonarConeLookup(step, theta[i]), a)
if a == 1:
a = 0.5
else:
a = 1
O.show()
from messages.udpMsg import SonarMsg
from ogrid.rawGrid import RawGrid
from oldTests.readLogFile import ReadCsvFile
from oldTests.readLogFile import ReadLogFile
csv = 0
if csv:
log = '/home/orjangr/Repos/pySonar/logs/UdpHubLog_4001_2017_11_02_09_00_03.csv'
log = 'logs/UdpHubLog_4001_2017_11_02_09_01_58.csv'
file = ReadCsvFile(log, 4002, 13102)
else:
log = 'logs/360 degree scan harbour piles.V4LOG'
file = ReadLogFile(log)
O = RawGrid(0.1, 20, 15, 0.5, True)
Threshold = 60
theta = np.zeros(1)
while file.messagesReturned < 2000:
msg = file.read_next_msg()
if type(msg) is SonarMsg and msg.type == 2:
print(file.messagesReturned)
O.auto_update_zhou(msg, Threshold)
theta = np.append(theta, msg.bearing)
elif msg == -1:
break
if file.messagesReturned % 5 == 0:
O.showP()
file.close()
#
print('Messages returned: %i\n' % file.messagesReturned)
from messages.udpMsg import SonarMsg
from ogrid.rawGrid import RawGrid
from oldTests.readLogFile import ReadCsvFile
from oldTests.readLogFile import ReadLogFile
csv = 0
if csv:
log = '/home/orjangr/Repos/pySonar/logs/UdpHubLog_4001_2017_11_02_09_00_03.csv'
log = 'logs/UdpHubLog_4001_2017_11_02_09_01_58.csv'
file = ReadCsvFile(log, 4002, 13102, cont_reading=False)
else:
log = '/home/orjangr/Repos/pySonar/logs/360 degree scan harbour piles.V4LOG'
file = ReadLogFile(log)
O = RawGrid(0.2, 20, 10, 0.5)
O2 = RawGrid(0.2, 20, 10, 0.5)
Threshold = 60
theta = np.zeros(1)
while file.messagesReturned < 10:
msg = file.read_next_msg()
if type(msg) is SonarMsg and msg.type == 2:
print(file.messagesReturned)
O.auto_update_zhou(msg, Threshold)
O2.auto_update_zhou(msg, Threshold)
theta = np.append(theta, msg.bearing)
elif msg == -1:
break
file.close()
ax = plt.subplot(131)
ax.set(xlabel='X [m]', ylabel='Y [m])')
import numpy as np
from messages.udpMsg import SonarMsg
from ogrid.rawGrid import RawGrid
from oldTests.readLogFile import ReadCsvFile
from oldTests.readLogFile import ReadLogFile
csv = 0
if csv:
log = '/home/orjangr/Repos/pySonar/logs/UdpHubLog_4001_2017_11_02_09_00_03.csv'
file = ReadCsvFile(log, 4002, 13102)
else:
log = 'logs/360 degree scan harbour piles.V4LOG'
file = ReadLogFile(log)
O = RawGrid(0.1, 20, 15, 0.5)
Threshold = 60
theta = np.array(1)
time = np.array(4.244359902158565 * 10**4)
fig, ax = plt.subplots()
while file.messagesReturned < 200:
msg = file.read_next_msg()
if type(msg) is SonarMsg:
theta = np.append(theta, msg.bearing)
time = np.append(time, msg.time)
ax.plot(range(0, np.shape(msg.data)[0]), msg.data)
elif msg == -1:
break
file.close()
import math
from ogrid.rawGrid import RawGrid
from oldTests.readLogFile import ReadCsvFile
from oldTests.readLogFile import ReadLogFile
csv = 0
if csv:
log = '/home/orjangr/Repos/pySonar/logs/UdpHubLog_4001_2017_11_02_09_00_03.csv'
file = ReadCsvFile(log, 4002, 13102)
else:
log = 'logs/360 degree scan harbour piles.V4LOG'
file = ReadLogFile(log)
O = RawGrid(0.1, 20, 15, 0.5)
O.coneSplit(O.sonarConeLookup(8 * O.GRAD2RAD, -math.pi / 3), 5)
O.show()
#
# def coneSplit(self, cone, rangeScale):
# subRange = cone[self.rHigh.flat[cone] < rangeScale - self.deltaSurface]
# onRange = cone[self.rLow.flat[cone] < (rangeScale + self.deltaSurface)]
# onRange = onRange[self.rHigh.flat[onRange] > (rangeScale - self.deltaSurface)]
# above = cone[self.rLow.flat[cone] >= (rangeScale + self.deltaSurface)]
# self.updateCells(subRange, 0.5)
# self.updateCells(onRange, 2)
# self.updateCells(above, 1)
# # print(subRange)
# # print(onRange)
# # print(above)
from ogrid.rawGrid import RawGrid
from matplotlib import pyplot as plt
import numpy as np
grid = RawGrid(False, 0.5)
grid.grid[200:1400,
200:1400] = np.random.random(np.shape(grid.grid[200:1400, 200:1400]))
plt.imshow(grid.grid.T)
plt.show()
grid.last_distance = 5
grid.update_distance(7)
plt.imshow(grid.grid.T)
plt.show()
#
# import numpy as np
# from scipy.interpolate import *
# from matplotlib import pyplot as plt
#
# last_distance = 15
# distance = 10
#
# x = np.linspace(0, 10, 11, True)/10.0
# xy_mesh_unit = np.meshgrid(x, x)
# grid = np.random.random((11, 11))
#
# old_x, old_y = np.multiply(xy_mesh_unit, last_distance)
# new_xy = x*distance
# tmp = np.array([old_x.ravel(), old_y.ravel()]).T
# new_grid = griddata(tmp, grid.ravel(), (new_xy,new_xy), method='linear', fill_value=5)
# # zfun_smooth_rbf = Rbf(old_xy, old_xy, grid, function='linear', smooth=0) # default s
from messages.udpMsg import SonarMsg
from ogrid.rawGrid import RawGrid
from oldTests.readLogFile import ReadCsvFile
from oldTests.readLogFile import ReadLogFile
csv = 0
if csv:
log = '/home/orjangr/Repos/pySonar/logs/UdpHubLog_4001_2017_11_02_09_00_03.csv'
log = 'logs/UdpHubLog_4001_2017_11_02_09_01_58.csv'
file = ReadCsvFile(log, 4002, 13102)
else:
log = 'logs/360 degree scan harbour piles.V4LOG'
file = ReadLogFile(log)
O = RawGrid(0.1, 20, 15, 0.5)
O2 = RawGrid(0.1, 20, 15, 0.5)
Threshold = 60
theta = np.zeros(1)
while file.messagesReturned < 50:
msg = file.read_next_msg()
if type(msg) is SonarMsg and msg.type == 2:
O.auto_update_zhou(msg, Threshold)
O2.auto_update_zhou(msg, Threshold)
theta = np.append(theta, msg.bearing)
elif msg == -1:
break
print('All equal before {}'.format(np.all(O.grid == O2.grid)))
t0 = time.time()
for i in range(0, 200):