""""Module to simulate Track filtering with loaded landmarks and vehicle pose from json files."""
from typing import Tuple
import csv
import json
import numpy as np
import numpy.typing as npt
import pandas as pd
from matplotlib.widgets import Slider, Button
import matplotlib.gridspec as gridspec
import matplotlib.pyplot as plt
from filtering_lib import TrackFiltering
[docs]class SLAM_LOADER():
def __init__(self, filepath_vehicle: str, filepath_landmarks: str):
self.filepath_vehicle = filepath_vehicle
self.filepath_landmarks = filepath_landmarks
self.load_vehicle_state_csv()
self.load_slam_landmarks()
self.get_landmarks(0)
offset = np.min([self.time[0], self.time_landmarks[0]])
self.time = self.time - offset
self.time_landmarks = self.time_landmarks - offset
[docs] def load_vehicle_state_csv(self):
with open(self.filepath_vehicle, 'r') as csv_file:
reader = csv.reader(csv_file)
next(reader)
fileinput = np.array(list(reader))
self.time = fileinput[:, 0].astype(float)
self.x_m = fileinput[:, 5].astype(float)
self.y_m = fileinput[:, 6].astype(float)
self.psi_rad = fileinput[:, 8].astype(float)
[docs] def load_slam_landmarks(self):
self.landmarks_array = pd.read_csv(self.filepath_landmarks)
self.time_landmarks = np.array(self.landmarks_array.iloc[:, 0])
[docs] def get_landmarks(self, index: int):
cones_dict = json.loads(self.landmarks_array.iloc[index]['cones'].
replace('x', '{"x"').
replace('y', '"y"').
replace('covariance', '"covariance"').
replace(']', ']}').
replace('\n', ',').
replace('id', '"id"')[:-1])
cones_array = np.array([[cone['x'], cone['y'], cone['id']] for cone in cones_dict]).astype(float)
self.left_cones = cones_array[cones_array[:, 2] == 1][:, :2]
self.right_cones = cones_array[cones_array[:, 2] == 0][:, :2]
self.orange_cones = cones_array[cones_array[:, 2] == 3][:, :2]
[docs] def find_closest(self, time: float):
dif_time = np.absolute(self.time - time)
dif_index = dif_time.argmin()
dif_time_landmarks = np.absolute(self.time_landmarks - time)
dif_index_landmarks = dif_time_landmarks.argmin()
return dif_index, dif_index_landmarks
if __name__ == '__main__':
My_Loader = SLAM_LOADER('../slam-vehicle_pose.csv', '../slam-landmarks.csv')
# HELPER
alpha = np.linspace(0, 2 * np.pi, 50)
colors = ['blue', 'orange', 'black', 'red']
# VARIABLES
local_fov_angle_rad = np.pi / 2
local_fov_range_m = 15
global_track = 0
time = 0
valid_time = np.arange(0, np.min([My_Loader.time[-1], My_Loader.time_landmarks[-1]]), 0.1)
valid_global_track = np.array([0, 1])
My_Filter = TrackFiltering(local_fov_angle_rad, local_fov_range_m)
def update_fov(local_fov_angle_rad: float, local_fov_range_m: float):
My_Filter.set_fov(local_fov_angle_rad, local_fov_range_m)
def get_output(left_cones: npt.NDArray, right_cones: npt.NDArray, orange_cones: npt.NDArray, x_m: float, y_m: float,
psi_rad: float, global_track: bool) -> Tuple[npt.NDArray, npt.NDArray, bool]:
if global_track == 0:
global_track = False
else:
global_track = True
My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m, y_m, psi_rad, global_track)
return My_Filter.centerpoints, My_Filter.track_widths
#################################
# PLOT #
#################################
gs = gridspec.GridSpec(2, 3)
plt.figure(figsize=(9, 9))
ax = plt.subplot(gs[:, :]) # row , col
index_time, index_time_landmarks = My_Loader.find_closest(time)
My_Loader.get_landmarks(index_time_landmarks)
centerpoints, track_widths = get_output(My_Loader.left_cones, My_Loader.right_cones,
My_Loader.orange_cones, My_Loader.x_m[index_time],
My_Loader.y_m[index_time], My_Loader.psi_rad[index_time], global_track)
X_1 = np.array([])
X_2 = np.array([])
Y_1 = np.array([])
Y_2 = np.array([])
for i in range(len(track_widths)):
x_1 = track_widths[i, 0] * np.cos(alpha) + centerpoints[i, 0]
x_2 = track_widths[i, 1] * np.cos(alpha) + centerpoints[i, 0]
y_1 = track_widths[i, 0] * np.sin(alpha) + centerpoints[i, 1]
y_2 = track_widths[i, 1] * np.sin(alpha) + centerpoints[i, 1]
X_1 = np.append(X_1, x_1)
X_2 = np.append(X_2, x_2)
Y_1 = np.append(Y_1, y_1)
Y_2 = np.append(Y_2, y_2)
line_0, = ax.plot(X_1, Y_1, '--', color='green', alpha=0.7)
line_1, = ax.plot(X_2, Y_2, '--', color='yellow', alpha=0.7)
line_2, = ax.plot(My_Loader.left_cones[:, 0], My_Loader.left_cones[:, 1], 'o', color=colors[0], label='left')
line_3, = ax.plot(My_Loader.right_cones[:, 0], My_Loader.right_cones[:, 1], 'o', color=colors[1], label='right')
line_4, = ax.plot(centerpoints[:, 0], centerpoints[:, 1], '.', color=colors[2], label='centerpoints')
line_5, = ax.plot(My_Loader.x_m[index_time], My_Loader.y_m[index_time], 'o', color=colors[3], label='vehicle pos')
line_6, = ax.plot(
[My_Loader.x_m[index_time], My_Loader.x_m[index_time] + np.cos(My_Loader.psi_rad[index_time]) * 3],
[My_Loader.y_m[index_time], My_Loader.y_m[index_time] + np.sin(My_Loader.psi_rad[index_time]) * 3],
'-', color=colors[3])
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_xlim([-30, 30])
ax.set_ylim([-30, 30])
ax.legend()
ax.grid()
text_runtime = plt.figtext(0.5, 0.9, f'runtime: {My_Filter.runtime} ms', fontsize=10)
# adjust the main plot to make room for the sliders
plt.subplots_adjust(left=0.1, bottom=0.20)
# SLIDER
#####################################################
# HOTSPOT #
#####################################################
axlocal_fov_angle_rad = plt.axes([0.2, 0.06, 0.4, 0.03])
local_fov_angle_rad_slider = Slider(
ax=axlocal_fov_angle_rad,
label='local_fov_angle_rad',
valmin=0,
valmax=2 * np.pi,
valinit=local_fov_angle_rad
)
axlocal_fov_range_m = plt.axes([0.2, 0.11, 0.4, 0.03])
local_fov_range_m_slider = Slider(
ax=axlocal_fov_range_m,
label='local_fov_range_m',
valmin=0,
valmax=50,
valinit=local_fov_range_m
)
axglobal_track = plt.axes([0.75, 0.11, 0.1, 0.03])
global_track_slider = Slider(
ax=axglobal_track,
label='global_track',
valmin=0,
valmax=1,
valinit=global_track,
valstep=valid_global_track
)
axiterator = plt.axes([0.2, 0.01, 0.65, 0.03])
iterator_slider = Slider(
ax=axiterator,
label='time [s]',
valmin=0,
valmax=valid_time[-1],
valstep=valid_time,
valinit=time
)
def update(val):
index_time, index_time_landmarks = My_Loader.find_closest(iterator_slider.val)
My_Loader.get_landmarks(index_time_landmarks)
My_Filter.set_fov(local_fov_angle_rad_slider.val, local_fov_range_m_slider.val)
centerpoints, track_widths = get_output(My_Loader.left_cones,
My_Loader.right_cones,
My_Loader.orange_cones,
My_Loader.x_m[index_time],
My_Loader.y_m[index_time],
My_Loader.psi_rad[index_time],
global_track_slider.val)
X_1 = np.array([])
X_2 = np.array([])
Y_1 = np.array([])
Y_2 = np.array([])
for i in range(len(track_widths)):
x_1 = track_widths[i, 0] * np.cos(alpha) + centerpoints[i, 0]
x_2 = track_widths[i, 1] * np.cos(alpha) + centerpoints[i, 0]
y_1 = track_widths[i, 0] * np.sin(alpha) + centerpoints[i, 1]
y_2 = track_widths[i, 1] * np.sin(alpha) + centerpoints[i, 1]
X_1 = np.append(X_1, x_1)
X_2 = np.append(X_2, x_2)
Y_1 = np.append(Y_1, y_1)
Y_2 = np.append(Y_2, y_2)
line_0.set_xdata(X_1)
line_0.set_ydata(Y_1)
line_1.set_xdata(X_2)
line_1.set_ydata(Y_2)
line_2.set_xdata(My_Loader.left_cones[:, 0])
line_2.set_ydata(My_Loader.left_cones[:, 1])
line_3.set_xdata(My_Loader.right_cones[:, 0])
line_3.set_ydata(My_Loader.right_cones[:, 1])
line_4.set_xdata(centerpoints[:, 0])
line_4.set_ydata(centerpoints[:, 1])
line_5.set_xdata(My_Loader.x_m[index_time])
line_5.set_ydata(My_Loader.y_m[index_time])
line_6.set_xdata([My_Loader.x_m[index_time],
My_Loader.x_m[index_time] + np.cos(My_Loader.psi_rad[index_time]) * 3])
line_6.set_ydata([My_Loader.y_m[index_time],
My_Loader.y_m[index_time] + np.sin(My_Loader.psi_rad[index_time]) * 3])
text_runtime.set_text(f'runtime: {My_Filter.runtime} ms')
if __name__ == '__main__':
# register the update function with each slider
local_fov_angle_rad_slider.on_changed(update)
local_fov_range_m_slider.on_changed(update)
global_track_slider.on_changed(update)
iterator_slider.on_changed(update)
# Create a `matplotlib.widgets.Button` to reset the sliders to initial values.
resetax = plt.axes([0.8, 0.9, 0.1, 0.04])
button = Button(resetax, 'Reset', hovercolor='0.975')
def reset(event):
local_fov_angle_rad_slider.reset()
local_fov_range_m_slider.reset()
global_track_slider.reset()
iterator_slider.reset()
button.on_clicked(reset)
plt.show()