From 67d0b8b51a50d2624a8b8d2631433b9e93f6b007 Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Tue, 23 Apr 2024 20:09:45 -0500 Subject: [PATCH] plotting particle filter data now --- .../include/autonav_filters/position_graph.py | 25 +++++++++++++++++++ autonav_ws/src/autonav_nav/src/astar.py | 2 +- 2 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py b/autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py new file mode 100644 index 00000000..391fceaa --- /dev/null +++ b/autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py @@ -0,0 +1,25 @@ +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + +if __name__ == "__main__": + my_csv = pd.read_csv('/home/tony/autonav_logs/1713916573060/autonav_filters_pf.csv', delimiter=',', skiprows=1) + + counter = 0 + for i in range(len(my_csv)): + + if my_csv.iloc[i, 1] == 0 or my_csv.iloc[i, 2] == 0: + print(my_csv.iloc[i, 1]) + print(my_csv.iloc[i, 2]) + counter += 1 + + print(counter) + my_csv = my_csv[counter:] + gps_x = my_csv.iloc[:, 1] + gps_y = my_csv.iloc[:, 2] + print(my_csv) + print(gps_x) + print(gps_y) + + plt.plot(gps_x, gps_y) + plt.show() \ No newline at end of file diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index d681161a..5657dc55 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -42,7 +42,7 @@ ] simulation_waypoints = [ - [(35.19505, -97.43823), (35.19492, -97.43824),(35.19485, -97.43824),(35.19471, -97.43823)], # Facing North + [(35.19510, -97.43823), (35.19505, -97.43823), (35.19492, -97.43824),(35.19485, -97.43824),(35.19471, -97.43824)], # Facing North [(35.19469, -97.43859), (35.19469, -97.43823),(35.19483, -97.43824),(35.19506, -97.43823)], # Facing South ]