"""Script to test local track filtering by generating test cases."""
import numpy as np
import pandas as pd
import json
from filtering_lib import TrackFiltering
[docs]def one_cone(side: str = 'left'):
if (side == 'left'):
left_cones = np.array([[2, 1.8]])
right_cones = np.array([])
else:
left_cones = np.array([])
right_cones = np.array([[2, -1.8]])
orange_cones = np.array([])
# plot
My_Filter = TrackFiltering(local_fov_angle_rad=np.pi, local_fov_range_m=20)
My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m=0, y_m=0, psi_rad=0, global_track=False)
My_Filter.draw()
[docs]def only_one_side(side: str = 'left'):
left_cones = np.array([[2.19, 6.24], [3.14, 6.28], [4.21, 6.27], [5.62, 6.17], [6.59, 5.7], [7.01, 4.88],
[7.04, 3.97], [6.92, 3.19], [6.38, 2.57], [5.53, 2.33], [4.52, 2.23], [3.56, 2.22],
[2.67, 2.25], [2.35, 2.26]])
right_cones = np.array([[2.26, 5.22], [3.47, 5.15], [4.72, 5.09], [5.27, 4.52], [5.39, 3.93], [5.09, 3.53],
[4.57, 3.42], [3.54, 3.41], [2.75, 3.43]])
left_cones = np.c_[left_cones[:5, 0] - 2, left_cones[:5, 1] - 5.73]
right_cones = np.c_[right_cones[:5, 0] - 2, right_cones[:5, 1] - 5.73]
left_cones *= 2.6
right_cones *= 2.6
if (side == 'left'):
right_cones = np.array([])
else:
left_cones = np.array([])
orange_cones = np.array([])
# plot
My_Filter = TrackFiltering(local_fov_angle_rad=np.pi, local_fov_range_m=20)
My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m=0, y_m=0, psi_rad=0, global_track=False)
My_Filter.draw()
[docs]def both_sides(l: int, r: int):
left_cones = np.array([[2.19, 6.24], [3.14, 6.28], [4.21, 6.27], [5.62, 6.17], [6.59, 5.7], [7.01, 4.88],
[7.04, 3.97], [6.92, 3.19], [6.38, 2.57], [5.53, 2.33], [4.52, 2.23], [3.56, 2.22],
[2.67, 2.25], [2.35, 2.26]])
right_cones = np.array([[2.26, 5.22], [3.47, 5.15], [4.72, 5.09], [5.27, 4.52], [5.39, 3.93], [5.09, 3.53],
[4.57, 3.42], [3.54, 3.41], [2.75, 3.43]])
left_cones = left_cones[:l]
right_cones = right_cones[:r]
left_cones = np.c_[left_cones[:, 0] - 2, left_cones[:, 1] - 5.73]
right_cones = np.c_[right_cones[:, 0] - 2, right_cones[:, 1] - 5.73]
left_cones *= 3.2
right_cones *= 3.2
left_cones = np.append(left_cones, np.array([[3, 3], [10, 4], [15.5, -13]])).reshape((-1, 2))
right_cones = np.append(right_cones, np.array([[2.5, -3], [6, -6], [7.5, -5]])).reshape((-1, 2))
orange_cones = np.array([])
# plot
My_Filter = TrackFiltering(local_fov_angle_rad=np.pi, local_fov_range_m=20)
My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m=0, y_m=0, psi_rad=0, global_track=False)
My_Filter.draw()
[docs]def gate():
left_cones = np.array([[3.14, 6.28]])
right_cones = np.array([[3.47, 5.15]])
left_cones[:, 0] -= 2
left_cones[:, 1] -= 5.73
right_cones[:, 0] -= 2
right_cones[:, 1] -= 5.73
left_cones *= 2.6
right_cones *= 2.6
orange_cones = np.array([])
# plot
My_Filter = TrackFiltering(local_fov_angle_rad=np.pi, local_fov_range_m=20)
My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m=0, y_m=0, psi_rad=0, global_track=False)
My_Filter.draw()
[docs]def curve(side: str = 'left'):
left_cones = np.array([[2.19, 6.24], [3.14, 6.28], [4.21, 6.27], [5.62, 6.17], [6.59, 5.7], [7.01, 4.88],
[7.04, 3.97], [6.92, 3.19], [6.38, 2.57], [5.53, 2.33], [4.52, 2.23], [3.56, 2.22],
[2.67, 2.25], [2.35, 2.26]])
right_cones = np.array([[2.26, 5.22], [3.47, 5.15], [4.72, 5.09], [5.27, 4.52], [5.39, 3.93], [5.09, 3.53],
[4.57, 3.42], [3.54, 3.41], [2.75, 3.43]])
left_cones = np.c_[left_cones[:5, 0] - 2, left_cones[:5, 1] - 5.73]
right_cones = np.c_[right_cones[:5, 0] - 2, right_cones[:5, 1] - 5.73]
left_cones *= 2.6
right_cones *= 2.6
orange_cones = np.array([])
if (side == 'left'):
right_cones = np.array([right_cones[2]])
else:
left_cones = np.array([left_cones[2]])
# plot
My_Filter = TrackFiltering(local_fov_angle_rad=np.pi, local_fov_range_m=20)
My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m=0, y_m=0, psi_rad=0, global_track=False)
My_Filter.draw()
# HELPER FUNCTIONS #
[docs]def load_map(filename: str):
"""
Load a gloabal map and return left and right array.
.. note:: Not started yet.
"""
pass
[docs]def get_vehicle_position_and_direction(filename: str, percentage: float):
"""
Get position and heading of vehicle for a calculated timestamp.
Timestamp will be calculated from percentage of maximum time.
.. note:: Not started yet.
"""
pass
#################################################################
if __name__ == '__main__':
landmarks_array = pd.read_csv('slam-landmarks.csv')
cones_dict = json.loads(landmarks_array.iloc[-1]['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])
left_cones = cones_array[cones_array[:, 2] == 1][:, :2]
right_cones = cones_array[cones_array[:, 2] == 0][:, :2]
orange_cones = cones_array[cones_array[:, 2] == 3][:, :2]
cones_dict = np.array(landmarks_array.iloc[:, 0])
# 1639143512.2382379,13428,1639143512,238237857,,-1.1243988928318844,8.167514799922676,8.568217354875479,2.7442074532309366
# My_Filter = TrackFiltering(local_fov_angle_rad=np.pi/2, local_fov_range_m=20)
# My_Filter.filtering_main(left_cones, right_cones, orange_cones, x_m=-1.1243988928318844, y_m=8.167514799922676, psi_rad=2.7442074532309366, global_track=True)
# My_Filter.draw()