diff --git a/uavf_2025/gnc/commander_node.py b/uavf_2025/gnc/commander_node.py index 3bc80e0b..bad27c74 100644 --- a/uavf_2025/gnc/commander_node.py +++ b/uavf_2025/gnc/commander_node.py @@ -275,16 +275,16 @@ def get_dropzone_bounds_mlocal(self) -> list[tuple[float, float]]: """ return [gps_to_local(x, self.global_home) for x in self.dropzone_bounds] - def get_roi_corners_local(self) -> list[tuple[float, float]]: + def get_roi_corners_local(self): """ Return the top left, top right, and bottom left corners of the ROI in local coordinates (in that order). """ - return [ + return ( gps_to_local(self.mapping_bounds[2], self.global_home), gps_to_local(self.mapping_bounds[3], self.global_home), gps_to_local(self.mapping_bounds[1], self.global_home), - ] + ) def get_pose_position(self) -> np.ndarray: """ diff --git a/uavf_2025/gnc/mapping_tsp.py b/uavf_2025/gnc/mapping_tsp.py index 9f06f846..eb7c1a23 100644 --- a/uavf_2025/gnc/mapping_tsp.py +++ b/uavf_2025/gnc/mapping_tsp.py @@ -1,13 +1,14 @@ -import cv2 import math +from pathlib import Path +from time import strftime + +import cv2 import networkx as nx import numpy as np -from pathlib import Path from PIL import Image -from time import strftime +from line_profiler import profile -HEATMAP_DIM = 1000 -FT_PER_METER = 3.28084 +HEATMAP_DIM = 100 class MappingPathPlanner: @@ -17,10 +18,13 @@ class MappingPathPlanner: def __init__( self, - heatmap: np.array, - roi_corners: tuple[tuple[float, float]], + heatmap: np.ndarray, + roi_corners: tuple[ + tuple[float, float], tuple[float, float], tuple[float, float] + ], m_height: float, min_unmapped: float, + ft_per_meter: float = 3.28084, ) -> None: """ heatmap: Grayscale image where black pixels indicate unmapped areas @@ -32,6 +36,8 @@ def __init__( self._roi_corners = roi_corners self._m_height = m_height self._min_unmapped = min_unmapped + self._ft_per_meter = ft_per_meter + self._G = nx.Graph() self._s = 0 self._local_path = [] @@ -42,23 +48,24 @@ def _initialize_graph_params(self) -> None: Calculate s value such that ROI is segmented into squares that are equal to or smaller than the area covered by the camera. """ - ft_height = self._m_height * FT_PER_METER + ft_height = self._m_height * self._ft_per_meter camera_y = 45 * ft_height / 50 x_ft_per_pixel = ( np.linalg.norm(np.subtract(self._roi_corners[0], self._roi_corners[1])) - * FT_PER_METER + * self._ft_per_meter ) / HEATMAP_DIM y_ft_per_pixel = ( np.linalg.norm(np.subtract(self._roi_corners[0], self._roi_corners[2])) - * FT_PER_METER + * self._ft_per_meter ) / HEATMAP_DIM camera_length = camera_y / max(x_ft_per_pixel, y_ft_per_pixel) self._s = int(camera_length / math.sqrt(2)) + @profile def _create_vertices(self) -> None: """ Vertices are in the format (x, y), which means that when represented in pixels @@ -80,8 +87,8 @@ def _create_vertices(self) -> None: (self._pixel_to_local((v_x, v_y))), heatmap_coord=(v_x, v_y) ) - def _divide_roi(self) -> None: - x_y_slices = [] + def _divide_roi(self): + x_y_slices: list[int] = [] x_y = self._s while x_y < HEATMAP_DIM: x_y_slices.append(x_y) @@ -90,6 +97,7 @@ def _divide_roi(self) -> None: return x_y_slices + @profile def _create_edges(self): V = list(self._G.nodes) total_V = len(V) @@ -99,13 +107,13 @@ def _create_edges(self): V[i], V[j], weight=np.linalg.norm(np.subtract(V[i], V[j])) ) - def _generate_tsp_cycle(self): + def _generate_tsp_cycle(self) -> list[tuple[float, float]]: return nx.approximation.traveling_salesman_problem( self._G, method=nx.approximation.christofides - ) + ) # type: ignore def _cycle_to_path( - self, current_pos: tuple[float], cycle: list[tuple[float, float]] + self, current_pos: tuple[float, ...], cycle: list[tuple[float, float]] ) -> list[tuple[float, float]]: """ Change Traveling Salesman Problem cycle to path starting at the vertex closest to the @@ -118,17 +126,18 @@ def _cycle_to_path( cycle.pop() return cycle[start_idx:] + cycle[0:start_idx] + @profile def construct_path( - self, current_pos: tuple[float] + self, current_pos: tuple[float, float] | tuple[float, float, float] ) -> list[tuple[float, float, float]]: self._create_vertices() if self._G.number_of_nodes() <= 1: self._local_path = list(self._G.nodes) else: self._create_edges() - self._local_path = self._cycle_to_path( - current_pos, self._generate_tsp_cycle() - ) + + cycle = self._generate_tsp_cycle() + self._local_path = self._cycle_to_path(current_pos, cycle) return list( map( lambda coord: (round(coord[0], 3), round(coord[1], 3), self._m_height), @@ -136,10 +145,11 @@ def construct_path( ) ) - def _pixel_to_local(self, px_coord: tuple[float, float]): + def _pixel_to_local(self, px_coord: tuple[float, float]) -> tuple[float, float]: """ Pixel coordinates are in form (column, row) to match the (x, y) format of vertices. """ + top_left = np.array(self._roi_corners[0]) horiz_dist = np.multiply( px_coord[0] / HEATMAP_DIM, np.subtract(self._roi_corners[1], self._roi_corners[0]), @@ -148,9 +158,9 @@ def _pixel_to_local(self, px_coord: tuple[float, float]): px_coord[1] / HEATMAP_DIM, np.subtract(self._roi_corners[2], self._roi_corners[0]), ) - return tuple(np.sum([self._roi_corners[0], horiz_dist, vert_dist], axis=0)) + return tuple(top_left + horiz_dist + vert_dist) - def _draw_vertices_on(self, img: np.array) -> None: + def _draw_vertices_on(self, img: np.ndarray) -> None: """ Row and column where vertices show up may be off by 0.5. """ @@ -159,7 +169,7 @@ def _draw_vertices_on(self, img: np.array) -> None: v = tuple(int(x) for x in v) cv2.line(img, v, v, (0, 255, 0), 20) - def _draw_tsp_edges_on(self, img: np.array) -> None: + def _draw_tsp_edges_on(self, img: np.ndarray) -> None: if len(self._local_path) <= 1: return for i in range(len(self._local_path) - 1): @@ -173,16 +183,16 @@ def _draw_tsp_edges_on(self, img: np.array) -> None: # Number last vertex in path cv2.putText(img, f"{i + 1}", v, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) - def draw_path(self, img: np.array) -> np.array: + def draw_path(self, img: np.ndarray) -> np.ndarray: img_dim = img.shape if len(img_dim) < 3 or img_dim[2] < 3: - img = Image.fromarray(img) - img = np.array(img.convert("RGB")) + img = np.array(Image.fromarray(img).convert("RGB")) + self._draw_vertices_on(img) self._draw_tsp_edges_on(img) return img - def save_path_image(self, img: np.array): + def save_path_image(self, img: np.ndarray): time_string = strftime("%Y-%m-%d_%H-%M") mapping_logs_path = Path(__file__).absolute().parent.parent.parent / Path( f"logs/mapping_path/{time_string}" @@ -191,3 +201,22 @@ def save_path_image(self, img: np.array): cv2.imwrite( str(mapping_logs_path / Path("planned_path.png")), self.draw_path(img) ) + + +if __name__ == "__main__": + # Example usage + heatmap = np.zeros((HEATMAP_DIM, HEATMAP_DIM), dtype=np.uint8) + roi_corners = ((0, 0), (HEATMAP_DIM, 0), (0, HEATMAP_DIM)) + m_height = 10.0 + min_unmapped = 0.1 + + planner = MappingPathPlanner(heatmap, roi_corners, m_height, min_unmapped) + current_position = (500, 500) # Example current position + path = planner.construct_path(current_position) + + import csv + + with open("mapping_path.csv", "w", newline="") as csvfile: + writer = csv.writer(csvfile) + writer.writerow(["x", "y", "z"]) + writer.writerows(path)