Skip to content

Commit

Permalink
Merge pull request #172 from oss-slu/issue165
Browse files Browse the repository at this point in the history 
Issue165
  • Loading branch information
@mohamdlog
mohamdlog authored Nov 19, 2024
2 parents 61d3cbd + 5cf08d8 commit cf7948a
Show file tree
Hide file tree
Showing 4 changed files with 160 additions and 165 deletions.
48 changes: 26 additions & 22 deletions backend/PythonClient/multirotor/monitor/circular_deviation_monitor.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ def update_position(self):
if not self.reported_breach:
self.reported_breach = True
self.append_fail_to_log(f"{self.target_drone};First breach: deviated more than "
f"{self.deviation_percentage}meter from the planned route")
f"{self.deviation_percentage} meter from the planned route")
self.breach_flag = True
self.est_position_array.append([x, y, z])
self.obj_position_array.append([ox, oy, oz])
Expand All @@ -103,12 +103,12 @@ def update_position(self):
# print(self.position_array)

def check_breach(self, x, y, z):
#print(f"Checking breach, Current position: {round(x, 2)}, {round(y, 2)}, {round(z, 2)}, "
# print(f"Checking breach, Current position: {round(x, 2)}, {round(y, 2)}, {round(z, 2)}, "
# f"center: {self.mission.center.x_val}, {self.mission.center.y_val}, {self.mission.altitude}")
return GeoUtil.is_point_close_to_circle([self.mission.center.x_val, self.mission.center.y_val, self.mission.altitude],
self.mission.radius,
[x, y, -z],
self.deviation_percentage)
self.mission.radius,
[x, y, -z],
self.deviation_percentage)

def calculate_actual_distance(self):
distance = 0.0
Expand All @@ -118,36 +118,40 @@ def calculate_actual_distance(self):

@staticmethod
def get_distance_btw_points(point_arr_1, point_arr_2):
return math.sqrt((point_arr_2[0] - point_arr_1[0]) ** 2 + (point_arr_2[1] - point_arr_1[1]) ** 2 + (
point_arr_2[2] - point_arr_1[2]) ** 2)
return math.sqrt((point_arr_2[0] - point_arr_1[0]) ** 2 +
(point_arr_2[1] - point_arr_1[1]) ** 2 +
(point_arr_2[2] - point_arr_1[2]) ** 2)

def draw_trace_3d(self):
graph_dir = self.get_graph_dir()
# Construct folder path
folder_path = f"{self.log_subdir}/{self.mission.__class__.__name__}/{self.__class__.__name__}/"
est_actual = self.est_position_array
# obj_actual = self.obj_position_array
radius = self.mission.radius
height = self.mission.altitude

if not self.breach_flag:
title = f"{self.target_drone} Planned vs. Actual\nDrone speed: {self.mission.speed} m/s\nWind: {self.wind_speed_text}"
else:
title = f"(FAILED) {self.target_drone} Planned vs. Actual\nDrone speed: {self.mission.speed} m/s\nWind: {self.wind_speed_text}"

center = [self.mission.center.x_val, self.mission.center.y_val, height]
theta = np.linspace(0, 2 * np.pi, 100)
x = center[0] + radius * np.cos(theta)
y = center[1] + radius * np.sin(theta)
z = np.ones(100) * height

planned = []
for i in range(len(x)):
planned.append([x[i], y[i], -z[i]])

ThreeDimensionalGrapher.draw_trace_vs_planned(planed_position_list=planned,
actual_position_list=est_actual,
full_target_directory=graph_dir,
drone_name=self.target_drone,
title=title)
ThreeDimensionalGrapher.draw_interactive_trace_vs_planned(planed_position_list=planned,
actual_position_list=est_actual,
full_target_directory=graph_dir,
drone_name=self.target_drone,
title=title)
planned = [[x[i], y[i], -z[i]] for i in range(len(x))]

# Use the grapher to draw and upload graphs
grapher = ThreeDimensionalGrapher(self.storage_service)
grapher.draw_trace_vs_planned(planed_position_list=planned,
actual_position_list=est_actual,
drone_name=self.target_drone,
title=title,
folder_path=folder_path)
grapher.draw_interactive_trace_vs_planned(planed_position_list=planned,
actual_position_list=est_actual,
drone_name=self.target_drone,
title=title,
folder_path=folder_path)
24 changes: 15 additions & 9 deletions backend/PythonClient/multirotor/monitor/drift_monitor.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ def start(self):

def make_drifted_array(self):
dt = self.dt
closest = 9223372036854775807 # Max int
closest = float('inf') # Maximum distance
self.reached = False
self.est_position_array = []
while self.mission.state != self.mission.State.END:
Expand All @@ -54,26 +54,32 @@ def make_drifted_array(self):
f"{round(self.threshold, 2)} meters. Closest distance: {round(closest, 2)} meters")

def draw_trace_3d(self):
# Construct folder path for storage service
folder_path = f"{self.log_subdir}/{self.mission.__class__.__name__}/{self.__class__.__name__}/"

actual = self.est_position_array
dest = self.mission.point

# Determine the title based on whether the target was reached
if self.reached:
title = f"Drift path\nDrone speed: {self.mission.speed} m/s\nWind: {self.wind_speed_text}\n" \
f"Closest distance: {round(self.closest,2)} meters"
f"Closest distance: {round(self.closest, 2)} meters"
else:
title = f"(FAILED) Drift path\nDrone speed: {self.mission.speed} m/s\nWind: {self.wind_speed_text}\n" \
f"Closest distance: {round(self.closest,2)} meters"
graph_dir = self.get_graph_dir()
grapher = ThreeDimensionalGrapher()
f"Closest distance: {round(self.closest, 2)} meters"

# Use the grapher to draw and upload graphs
grapher = ThreeDimensionalGrapher(self.storage_service)
grapher.draw_trace_vs_point(destination_point=dest,
actual_position_list=actual,
full_target_directory=graph_dir,
drone_name=self.target_drone,
title=title)
title=title,
folder_path=folder_path)
grapher.draw_interactive_trace_vs_point(actual_position=actual,
destination=dest,
full_target_directory=graph_dir,
drone_name=self.target_drone,
title=title)
title=title,
folder_path=folder_path)


if __name__ == "__main__":
Expand Down
37 changes: 22 additions & 15 deletions backend/PythonClient/multirotor/monitor/point_deviation_monitor.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,22 +124,29 @@ def calculate_actual_distance(self):
return distance

def draw_trace_3d(self):
graph_dir = self.get_graph_dir()
# Construct the folder path
folder_path = f"{self.log_subdir}/{self.mission.__class__.__name__}/{self.__class__.__name__}/"

# Ensure the title reflects the mission status
if not self.breach_flag:
title = f"{self.target_drone} Planned vs. Actual\nDrone speed: {self.mission.speed} m/s\nWind: {self.wind_speed_text}"
else:
title = f"(FAILED) {self.target_drone} Planned vs. Actual\nDrone speed: {self.mission.speed} m/s\nWind: {self.wind_speed_text}"
grapher = ThreeDimensionalGrapher()
grapher.draw_trace_vs_planned(planed_position_list=self.mission.points,
actual_position_list=self.est_position_array,
full_target_directory=graph_dir,
drone_name=self.target_drone,
title=title
)

grapher.draw_interactive_trace_vs_planned(planed_position_list=self.mission.points,
actual_position_list=self.est_position_array,
full_target_directory=graph_dir,
drone_name=self.target_drone,
title=title
)

# Draw and upload the graphs
grapher = ThreeDimensionalGrapher(self.storage_service)
grapher.draw_trace_vs_planned(
planed_position_list=self.mission.points,
actual_position_list=self.est_position_array,
drone_name=self.target_drone,
title=title,
folder_path=folder_path
)

grapher.draw_interactive_trace_vs_planned(
planed_position_list=self.mission.points,
actual_position_list=self.est_position_array,
drone_name=self.target_drone,
title=title,
folder_path=folder_path
)
Loading

0 comments on commit cf7948a

Please sign in to comment.