From 5edb7aa2aba68f9feecafc15acd853939f92e26b Mon Sep 17 00:00:00 2001 From: "Looks good to me... :D" Date: Wed, 23 Jul 2025 13:28:36 -0400 Subject: [PATCH] Seed costmap with CanElevation data --- setup/install_nav_deps.sh | 5 +- .../can_elevation_downloader_params.yaml | 14 + .../costmap_seeding/__init__.py | 0 .../download_can_elevation_maps.py | 1054 +++++++++++++++++ .../costmap_seeding/fake_gps_publisher.py | 39 + .../load_can_elevation_maps.py | 807 +++++++++++++ .../launch/download_can_elevation.launch.py | 37 + src/costmap_seeding/package.xml | 18 + src/costmap_seeding/resource/costmap_seeding | 0 src/costmap_seeding/setup.cfg | 4 + src/costmap_seeding/setup.py | 30 + src/costmap_seeding/test/test_copyright.py | 27 + src/costmap_seeding/test/test_flake8.py | 25 + src/costmap_seeding/test/test_pep257.py | 23 + 14 files changed, 2082 insertions(+), 1 deletion(-) create mode 100644 src/costmap_seeding/config/can_elevation_downloader_params.yaml create mode 100644 src/costmap_seeding/costmap_seeding/__init__.py create mode 100644 src/costmap_seeding/costmap_seeding/download_can_elevation_maps.py create mode 100644 src/costmap_seeding/costmap_seeding/fake_gps_publisher.py create mode 100644 src/costmap_seeding/costmap_seeding/load_can_elevation_maps.py create mode 100644 src/costmap_seeding/launch/download_can_elevation.launch.py create mode 100644 src/costmap_seeding/package.xml create mode 100644 src/costmap_seeding/resource/costmap_seeding create mode 100644 src/costmap_seeding/setup.cfg create mode 100644 src/costmap_seeding/setup.py create mode 100644 src/costmap_seeding/test/test_copyright.py create mode 100644 src/costmap_seeding/test/test_flake8.py create mode 100644 src/costmap_seeding/test/test_pep257.py diff --git a/setup/install_nav_deps.sh b/setup/install_nav_deps.sh index a625e320..17e3c089 100755 --- a/setup/install_nav_deps.sh +++ b/setup/install_nav_deps.sh @@ -19,4 +19,7 @@ mkdir build cd build cmake .. -DUSE_CMAKE=true sudo make install -echo "Finished building Kindr ..." \ No newline at end of file +echo "Finished building Kindr ..." + +sudo mkdir -p /usr/local/cprt +sudo chown $USER:$USER /usr/local/cprt \ No newline at end of file diff --git a/src/costmap_seeding/config/can_elevation_downloader_params.yaml b/src/costmap_seeding/config/can_elevation_downloader_params.yaml new file mode 100644 index 00000000..bbbd0724 --- /dev/null +++ b/src/costmap_seeding/config/can_elevation_downloader_params.yaml @@ -0,0 +1,14 @@ +can_elevation_downloader_node: + ros__parameters: + dataset_parent_dir: "/usr/local/cprt" + download_dtms: false + dataset_names: ["Drumheller", "Ottawa_Gatineau"] + Drumheller_url: "https://ftp.maps.canada.ca/pub/elevation/dem_mne/highresolution_hauteresolution/dtm_mnt/1m/AB/Drumheller/" + Ottawa_Gatineau_url: "https://ftp.maps.canada.ca/pub/elevation/dem_mne/highresolution_hauteresolution/dsm_mns/1m/VILLE_OTTAWA/Ottawa_Gatineau_2020/" + target_map_resolution_meters: 1.0 + max_chunk_dimension: 6000 + slope_weight: 0.7 + roughness_weight: 0.3 + max_slope_deg: 25.0 + max_roughness_m: 0.75 + roughness_blur_sigma: 4.0 diff --git a/src/costmap_seeding/costmap_seeding/__init__.py b/src/costmap_seeding/costmap_seeding/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/costmap_seeding/costmap_seeding/download_can_elevation_maps.py b/src/costmap_seeding/costmap_seeding/download_can_elevation_maps.py new file mode 100644 index 00000000..c52e68b1 --- /dev/null +++ b/src/costmap_seeding/costmap_seeding/download_can_elevation_maps.py @@ -0,0 +1,1054 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from std_srvs.srv import Trigger # A simple service for triggering operations + +import requests +import os +from bs4 import BeautifulSoup +import rasterio +from rasterio.transform import Affine # Explicitly import Affine +import folium +import zipfile +import xml.etree.ElementTree as ET +import shutil +import yaml # For YAML file handling + +# Updated imports for coordinate transformations +from rasterio.crs import CRS +from pyproj import Transformer # Import Transformer from pyproj + +import matplotlib.pyplot as plt +import numpy as np + +# Import for 3D plotting +from mpl_toolkits.mplot3d import Axes3D +from matplotlib import cm # Colormap module +import scipy.ndimage # Import for image processing operations (blur, zoom) +import math # For math.ceil +import traceback + +# --- Utility Functions (now methods of CanElevationDownloader) --- + + +class CanElevationDownloader(Node): # Renamed node + def __init__(self): + super().__init__("can_elevation_downloader_node") # Renamed node + self.get_logger().info("CanElevationDownloader Node starting...") + + # Declare ROS2 parameter for the parent directory of datasets + self.declare_parameter("dataset_parent_dir", "/usr/local/cprt") + self.dataset_parent_dir = ( + self.get_parameter("dataset_parent_dir").get_parameter_value().string_value + ) + + # Check if the dataset_parent_dir exists and has usable permissions + if not os.path.exists(self.dataset_parent_dir): + self.get_logger().error( + f"Base directory '{self.dataset_parent_dir}' does not exist. Please create it and ensure permissions." + ) + # Depending on desired behavior, you might want to raise an exception or rclpy.shutdown() here + elif not os.access( + self.dataset_parent_dir, os.W_OK | os.X_OK + ): # Check for write and execute permissions + self.get_logger().error( + f"Base directory '{self.dataset_parent_dir}' does not have sufficient write/execute permissions. Please adjust permissions." + ) + # Depending on desired behavior, you might want to raise an exception or rclpy.shutdown() here + else: + self.get_logger().info( + f"Base directory '{self.dataset_parent_dir}' exists and has usable permissions." + ) + + # Set the full data base directory based on the parameter + self.data_base_dir = os.path.join( + self.dataset_parent_dir, "elevation_datasets", "CanElevation" + ) + os.makedirs( + self.data_base_dir, exist_ok=True + ) # Ensure the full data base directory exists + self.get_logger().info(f"Data base directory set to: {self.data_base_dir}") + + # Declare remaining ROS2 parameters + self.declare_parameter("download_dtms", False) + self.declare_parameter( + "dataset_names", ["Drumheller"] + ) # Changed to 'Drumheller' + self.declare_parameter( + "Drumheller_url", + "https://ftp.maps.canada.ca/pub/elevation/dem_mne/highresolution_hauteresolution/dtm_mnt/1m/AB/Drumheller/", + ) # Changed parameter name + + # Add parameters for calculate_traversability_cost + self.declare_parameter("target_map_resolution_meters", 1.0) # Renamed parameter + self.declare_parameter( + "max_chunk_dimension", 6000 + ) # New parameter for max chunk size + self.declare_parameter("slope_weight", 0.7) + self.declare_parameter("roughness_weight", 0.3) + self.declare_parameter("max_slope_deg", 25.0) + self.declare_parameter("max_roughness_m", 0.75) + self.declare_parameter("roughness_blur_sigma", 4.0) + + # Retrieve remaining ROS2 parameters + self.download_dtms = ( + self.get_parameter("download_dtms").get_parameter_value().bool_value + ) + self.dataset_names = ( + self.get_parameter("dataset_names").get_parameter_value().string_array_value + ) + self.get_logger().info( + f"Dataset names set to: {self.dataset_names}. Download DTMs on startup: {self.download_dtms}" + ) + + # Retrieve new parameters for costmap calculation + self.target_map_resolution_meters = ( + self.get_parameter("target_map_resolution_meters") + .get_parameter_value() + .double_value + ) + self.max_chunk_dimension = ( + self.get_parameter("max_chunk_dimension") + .get_parameter_value() + .integer_value + ) + self.slope_weight = ( + self.get_parameter("slope_weight").get_parameter_value().double_value + ) + self.roughness_weight = ( + self.get_parameter("roughness_weight").get_parameter_value().double_value + ) + self.max_slope_deg = ( + self.get_parameter("max_slope_deg").get_parameter_value().double_value + ) + self.max_roughness_m = ( + self.get_parameter("max_roughness_m").get_parameter_value().double_value + ) + self.roughness_blur_sigma = ( + self.get_parameter("roughness_blur_sigma") + .get_parameter_value() + .double_value + ) + + # Configure QoS for services + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + # durability=DurabilityPolicy.TRANSIENT_LOCAL # Essential for services + ) + + self.generate_costmaps_srv = self.create_service( + Trigger, + "generate_elevation_costmaps", + self.generate_elevation_costmaps_callback, + qos_profile=qos_profile, + ) + self.get_logger().info('Service "/generate_elevation_costmaps" is ready.') + + # If download_dtms parameter is true, initiate download on node startup + if self.download_dtms: + self.get_logger().info( + 'Parameter "download_dtms" is true. Initiating all DTM downloads.' + ) + self.download_all_dtms() + else: + self.get_logger().info( + 'Parameter "download_dtms" is false. Skipping DTM downloads on startup.' + ) + + def _download_file_from_url(self, url, destination_folder): + """ + Downloads a single file from a given URL to a specified folder. + This version does not include a progress bar, only basic status messages. + + Args: + url (str): The URL of the file to download. + destination_folder (str): The local folder where the file will be saved. + It will be created if it doesn't exist. + + Returns: + str: The full path to the downloaded file, or None if download fails. + """ + try: + # Ensure the destination folder exists + if not os.path.exists(destination_folder): + os.makedirs(destination_folder) + + # Extract filename from the URL + filename = os.path.join(destination_folder, url.split("/")[-1]) + + if os.path.exists(filename): + self.get_logger().info( + f"File '{filename}' already exists. Skipping download." + ) + return filename + + self.get_logger().info(f"Downloading: {url}") + response = requests.get(url, stream=True) + response.raise_for_status() # Raise an exception for bad status codes (4xx or 5xx) + + with open(filename, "wb") as f: + for chunk in response.iter_content(chunk_size=1024): + if chunk: # filter out keep-alive new chunks + f.write(chunk) + + self.get_logger().info(f"Successfully downloaded '{filename}'") + return filename + + except requests.exceptions.RequestException as e: + self.get_logger().error(f"Error downloading {url}: {e}") + return None + except Exception as e: + self.get_logger().error( + f"An unexpected error occurred during download of {filename}: {e}" + ) + return None + + def _list_dtm_urls_from_kml( + self, + base_directory_url, + kml_filename="INDEX_utm12_AB_Drumheller.kmz", + local_kml_path="kml", + ): + """ + Downloads and parses a KMZ file from the base directory to extract DTM .tif URLs. + + Args: + base_directory_url (str): The base URL containing the KMZ file. + kml_filename (str): The name of the KMZ file. + local_kml_path (str): Folder to store the KMZ and its extracted content. + + Returns: + list: A list of absolute URLs to DTM .tif files. + """ + dtm_urls = [] + kml_url = requests.compat.urljoin(base_directory_url, kml_filename) + + # Ensure KML folder exists + if not os.path.exists(local_kml_path): + os.makedirs(local_kml_path) + + # 1. Download the KMZ file + kmz_local_path = self._download_file_from_url(kml_url, local_kml_path) + if not kmz_local_path: + self.get_logger().error(f"Failed to download KMZ file from {kml_url}") + return [] + + # 2. Extract KML from KMZ + kml_extracted_path = None + try: + with zipfile.ZipFile(kmz_local_path, "r") as zip_ref: + # KMZ files usually contain a 'doc.kml' or similar at the root + kml_files = [ + f for f in zip_ref.namelist() if f.lower().endswith(".kml") + ] + if not kml_files: + self.get_logger().warning( + f"No KML file found inside {kmz_local_path}" + ) + return [] + + # Assuming the main KML file is the first one found (often 'doc.kml') + kml_file_in_zip = kml_files[0] + zip_ref.extract(kml_file_in_zip, local_kml_path) + kml_extracted_path = os.path.join(local_kml_path, kml_file_in_zip) + self.get_logger().info(f"Extracted KML to: {kml_extracted_path}") + except zipfile.BadZipFile: + self.get_logger().error(f"Error: {kmz_local_path} is not a valid KMZ file.") + return [] + except Exception as e: + self.get_logger().error(f"Error extracting KML from KMZ: {e}") + return [] + + # 3. Parse the KML file to find DTM links + if kml_extracted_path: + try: + tree = ET.parse(kml_extracted_path) + root = tree.getroot() + + # KML uses XML namespaces. Define the default KML namespace. + ns = {"kml": "http://www.opengis.net/kml/2.2"} + + # Iterate through all Placemark elements + for placemark in root.findall(".//kml:Placemark", ns): + description_element = placemark.find("kml:description", ns) + if description_element is not None and description_element.text: + # The description contains CDATA with HTML links. Use BeautifulSoup to parse it. + description_html = description_element.text + soup = BeautifulSoup(description_html, "html.parser") + + # Find the tag whose text is "Digital Terrain Model" or its French equivalent + for a_tag in soup.find_all("a", href=True): + link_text = a_tag.get_text(strip=True) + if ( + "Digital Terrain Model" in link_text + or "Modèle numérique de terrain" in link_text + ): + dtm_url = a_tag["href"] + if dtm_url: + dtm_urls.append(dtm_url) + break # Found the DTM link for this placemark, move to the next placemark + except ET.ParseError as e: + self.get_logger().error( + f"Error parsing KML file {kml_extracted_path}: {e}" + ) + except Exception as e: + self.get_logger().error( + f"An unexpected error occurred while parsing KML: {e}" + ) + + return dtm_urls + + def _download_dtm_data_from_kml_index( + self, base_data_directory_url, local_kml_folder, output_dtm_folder + ): + """ + Downloads all available DTM GeoTIFF files based on a KML index. + Automatically discovers the .kmz file in the base_data_directory_url. + + Args: + base_directory_url (str): The base URL containing the KMZ index file. + local_kml_folder (str): Folder to store the KMZ and its extracted KML content (will not be deleted). + output_dtm_folder (str): The local folder to save all downloaded GeoTIFFs. + + Returns: + list: A list of file paths to the successfully downloaded DTM GeoTIFFs. + """ + # 1. Discover the KMZ file from the base_data_directory_url + kmz_filename = None + try: + response = requests.get(base_data_directory_url) + response.raise_for_status() + soup = BeautifulSoup(response.text, "html.parser") + for link in soup.find_all("a", href=True): + href = link["href"] + if href.lower().endswith(".kmz"): + kmz_filename = href + self.get_logger().info(f"Discovered KMZ file: {kmz_filename}") + break + if not kmz_filename: + self.get_logger().error( + f"No .kmz file found at {base_data_directory_url}. Cannot proceed with DTM download." + ) + return [] + except requests.exceptions.RequestException as e: + self.get_logger().error( + f"Error accessing {base_data_directory_url} to find KMZ: {e}" + ) + return [] + except Exception as e: + self.get_logger().error( + f"An unexpected error occurred while discovering KMZ: {e}" + ) + return [] + + all_dtm_tif_urls = self._list_dtm_urls_from_kml( + base_data_directory_url, + kml_filename=kmz_filename, # Pass the discovered KMZ filename + local_kml_path=local_kml_folder, + ) + + if not all_dtm_tif_urls: + self.get_logger().warning( + "No DTM .tif URLs found from KMZ. Cannot download any data." + ) + return [] + + self.get_logger().info( + f"Found {len(all_dtm_tif_urls)} DTM .tif URLs. Starting download of all files." + ) + + downloaded_filepaths = [] + all_dtm_tif_urls.sort() # Ensure consistent download order + for tif_url in all_dtm_tif_urls: + downloaded_path = self._download_file_from_url(tif_url, output_dtm_folder) + if downloaded_path: + downloaded_filepaths.append(downloaded_path) + + return downloaded_filepaths + + def _calculate_traversability_cost( + self, + elevation_data_2d, + original_resolution_m, + target_resolution_m, + original_transform, # New parameter + original_crs, # New parameter + max_chunk_dimension, # ROS2 parameter + slope_weight=0.5, + roughness_weight=0.5, + max_slope_deg=20.0, # Maximum traversable slope in degrees + max_roughness_m=0.5, # Maximum traversable roughness in meters + roughness_blur_sigma=1.0, # Sigma for Gaussian blur for roughness calculation (in pixels) + ): + """ + Calculates a 2D traversability cost array (0.0 to 1.0) from elevation data. + Processes large arrays in chunks to manage memory. + Shrinks the array based on valid data before processing. + + Args: + elevation_data_2d (np.ndarray): The input 2D NumPy array of elevation data (e.g., 1m resolution). + original_resolution_m (float): The spatial resolution of the input elevation data in meters. + target_resolution_m (float): The desired spatial resolution for the output cost map in meters. + original_transform (rasterio.transform.Affine): The affine transform of the original elevation_data_2d. + original_crs (rasterio.crs.CRS): The CRS of the original elevation_data_2d. + max_chunk_dimension (int): The maximum dimension (width or height) for a processing chunk. + slope_weight (float): Weight for the slope cost component (0.0 to 1.0). + roughness_weight (float): Weight for the roughness cost component (0.0 to 1.0). + max_slope_deg (float): Slope in degrees above which traversability becomes very high cost (1.0). + max_roughness_m (float): Roughness in meters above which traversability becomes very high cost (1.0). + roughness_blur_sigma (float): Standard deviation for the Gaussian filter used to smooth elevations + for roughness calculation. In original pixel units. + + Returns: + tuple: (np.ndarray, Affine, CRS) + A 2D NumPy array representing the traversability cost (0.0 to 1.0), + interpolated to the target resolution, along with the adjusted Affine transform + and the original CRS. Returns (None, None, None) if input is invalid or + contains no valid data. + """ + if elevation_data_2d is None or elevation_data_2d.size == 0: + self.get_logger().error( + "Input elevation data is empty or invalid for cost calculation." + ) + return None, None, None + + # 1. Find the bounding box of valid (non-NaN) data + non_nan_rows = np.where(~np.all(np.isnan(elevation_data_2d), axis=1))[0] + non_nan_cols = np.where(~np.all(np.isnan(elevation_data_2d), axis=0))[0] + + if non_nan_rows.size == 0 or non_nan_cols.size == 0: + self.get_logger().warning( + "Input elevation data contains only NaN values. No costmap will be generated with valid data." + ) + # Create a 1x1 array of NaN, which write_elevation_costmap will handle as 255 (unknown) + output_cost_array = np.full((1, 1), np.nan, dtype=np.float32) + + # Create a minimal transform for this 1x1 NaN array + # Center the 1x1 at the original array's center for best representation if all unknown + center_x_orig, center_y_orig = original_transform * ( + elevation_data_2d.shape[1] / 2, + elevation_data_2d.shape[0] / 2, + ) + + # Calculate new top-left for 1x1 array that would place its center at original array's center + new_top_left_x = center_x_orig - (original_resolution_m / 2) + new_top_left_y = center_y_orig + ( + original_resolution_m / 2 + ) # Y increases upwards, so add to top-left + + adjusted_transform = Affine( + original_resolution_m, + 0, + new_top_left_x, + 0, + -original_resolution_m, + new_top_left_y, + ) + + return output_cost_array, adjusted_transform, original_crs + + min_row_crop = non_nan_rows[0] + max_row_crop = non_nan_rows[-1] + min_col_crop = non_nan_cols[0] + max_col_crop = non_nan_cols[-1] + + # Crop the elevation data to the bounding box + elevation_data_cropped = elevation_data_2d[ + min_row_crop : max_row_crop + 1, min_col_crop : max_col_crop + 1 + ] + + # Calculate the top-left coordinate of the cropped area in the original CRS (UTM) + new_top_left_utm_x, new_top_left_utm_y = original_transform * ( + min_col_crop, + min_row_crop, + ) + + # Create the adjusted transform for the cropped array + adjusted_transform = Affine( + original_resolution_m, + 0, + new_top_left_utm_x, + 0, + -original_resolution_m, + new_top_left_utm_y, + ) + + rows, cols = elevation_data_cropped.shape + self.get_logger().info( + f"Elevation data cropped to {rows}x{cols} from original. Proceeding with calculations on cropped data." + ) + + # Determine padding needed for kernel operations (gradient and gaussian blur) + PAD_PIXELS = int(math.ceil(5 * roughness_blur_sigma)) + if PAD_PIXELS == 0: # Ensure minimum padding for gradient + PAD_PIXELS = 1 + + # Use the max_chunk_dimension from parameters + # Ensure chunk dimension is large enough to accommodate padding + if max_chunk_dimension < 2 * PAD_PIXELS + 1: + # Fallback to a safe minimum if parameter is too small + MAX_CHUNK_DIM_EFFECTIVE = 2 * PAD_PIXELS + 1 + self.get_logger().warning( + f"max_chunk_dimension ({max_chunk_dimension}) is too small for padding ({PAD_PIXELS}). Using effective chunk dimension of {MAX_CHUNK_DIM_EFFECTIVE}." + ) + else: + MAX_CHUNK_DIM_EFFECTIVE = max_chunk_dimension + + # Check if chunking is necessary + if rows * cols > MAX_CHUNK_DIM_EFFECTIVE * MAX_CHUNK_DIM_EFFECTIVE: + self.get_logger().info( + f"Cropped array size ({rows}x{cols}) exceeds max chunk size ({MAX_CHUNK_DIM_EFFECTIVE}x{MAX_CHUNK_DIM_EFFECTIVE}). Processing in chunks." + ) + + # Initialize output cost array at original resolution + combined_cost_original_res = np.empty_like( + elevation_data_cropped, dtype=np.float32 + ) + + # Calculate step size for iteration, ensuring overlap for padding + effective_chunk_dim = MAX_CHUNK_DIM_EFFECTIVE - 2 * PAD_PIXELS + if effective_chunk_dim <= 0: + self.get_logger().error( + "Error: Effective chunk dimension is too small after accounting for padding. Increase max_chunk_dimension or decrease roughness_blur_sigma." + ) + return None, None, None + + for r_start in range(0, rows, effective_chunk_dim): + for c_start in range(0, cols, effective_chunk_dim): + # Define the actual slice for the output (without padding) + r_end = min(r_start + effective_chunk_dim, rows) + c_end = min(c_start + effective_chunk_dim, cols) + + # Define the padded slice for input (extending beyond actual slice) + padded_r_start = max(0, r_start - PAD_PIXELS) + padded_r_end = min(rows, r_end + PAD_PIXELS) + padded_c_start = max(0, c_start - PAD_PIXELS) + padded_c_end = min(cols, c_end + PAD_PIXELS) + + # Extract the padded chunk + current_elevation_chunk = elevation_data_cropped[ + padded_r_start:padded_r_end, padded_c_start:padded_c_end + ] + + # If a chunk is entirely outside the valid area (e.g., due to min/max clipping causing small overlap) + if current_elevation_chunk.size == 0: + continue + + # Calculate slope for the chunk + dz_dy_chunk, dz_dx_chunk = np.gradient( + current_elevation_chunk, original_resolution_m + ) + slope_radians_chunk = np.arctan( + np.sqrt(dz_dx_chunk**2 + dz_dy_chunk**2) + ) + slope_degrees_chunk = np.degrees(slope_radians_chunk) + slope_cost_chunk = np.clip( + slope_degrees_chunk / max_slope_deg, 0.0, 1.0 + ) + + # Calculate roughness for the chunk + smoothed_elevation_chunk = scipy.ndimage.gaussian_filter( + current_elevation_chunk, + sigma=roughness_blur_sigma, + mode="constant", + cval=np.nan, + ) + roughness_chunk = np.abs( + current_elevation_chunk - smoothed_elevation_chunk + ) + roughness_cost_chunk = np.clip( + roughness_chunk / max_roughness_m, 0.0, 1.0 + ) + + # Combine costs for the chunk + total_weight = slope_weight + roughness_weight + if total_weight == 0: + chunk_combined_cost = np.zeros_like(slope_cost_chunk) + else: + normalized_slope_weight = slope_weight / total_weight + normalized_roughness_weight = roughness_weight / total_weight + chunk_combined_cost = ( + normalized_slope_weight * slope_cost_chunk + ) + (normalized_roughness_weight * roughness_cost_chunk) + + # Crop the chunk_combined_cost to remove padding and place into the main array + crop_r_start = r_start - padded_r_start + crop_c_start = c_start - padded_c_start + crop_r_end = crop_r_start + (r_end - r_start) + crop_c_end = crop_c_start + (c_end - c_start) + + combined_cost_original_res[r_start:r_end, c_start:c_end] = ( + chunk_combined_cost[ + crop_r_start:crop_r_end, crop_c_start:crop_c_end + ] + ) + self.get_logger().info( + f"Cost map generated in chunks at original {original_resolution_m}m resolution." + ) + else: # No chunking needed, process the whole array directly + self.get_logger().info( + f"Cropped array size ({rows}x{cols}) is within max chunk size. Processing as a whole." + ) + # 1. Calculate Slope + dz_dy, dz_dx = np.gradient(elevation_data_cropped, original_resolution_m) + slope_radians = np.arctan(np.sqrt(dz_dx**2 + dz_dy**2)) + slope_degrees = np.degrees(slope_radians) + slope_cost = np.clip(slope_degrees / max_slope_deg, 0.0, 1.0) + self.get_logger().info( + f"Slope cost calculated. Min: {np.nanmin(slope_cost) if not np.all(np.isnan(slope_cost)) else 'NaN' :.2f}, Max: {np.nanmax(slope_cost) if not np.all(np.isnan(slope_cost)) else 'NaN' :.2f}" + ) + + # 2. Calculate Roughness + smoothed_elevation = scipy.ndimage.gaussian_filter( + elevation_data_cropped, + sigma=roughness_blur_sigma, + mode="constant", + cval=np.nan, + ) + roughness = np.abs(elevation_data_cropped - smoothed_elevation) + roughness_cost = np.clip(roughness / max_roughness_m, 0.0, 1.0) + self.get_logger().info( + f"Roughness cost calculated. Min: {np.nanmin(roughness_cost) if not np.all(np.isnan(roughness_cost)) else 'NaN' :.2f}, Max: {np.nanmax(roughness_cost) if not np.all(np.isnan(roughness_cost)) else 'NaN' :.2f}" + ) + + # 3. Combine the cost of slope with the cost of roughness + total_weight = slope_weight + roughness_weight + if total_weight == 0: + self.get_logger().warning( + "Warning: Both slope_weight and roughness_weight are zero. Returning zero cost map with NaNs retained." + ) + combined_cost_original_res = np.zeros_like( + slope_cost + ) # Still need to apply NaN mask later + else: + normalized_slope_weight = slope_weight / total_weight + normalized_roughness_weight = roughness_weight / total_weight + combined_cost_original_res = (normalized_slope_weight * slope_cost) + ( + normalized_roughness_weight * roughness_cost + ) + + # 4. Limit the values to be within 0 to 1 (NaNs remain NaN) + combined_cost_original_res = np.clip(combined_cost_original_res, 0.0, 1.0) + self.get_logger().info( + f"Final combined cost (at original resolution). Min: {np.nanmin(combined_cost_original_res) if not np.all(np.isnan(combined_cost_original_res)) else 'NaN' :.2f}, Max: {np.nanmax(combined_cost_original_res) if not np.all(np.isnan(combined_cost_original_res)) else 'NaN' :.2f}" + ) + + # 5. Interpolate if target_resolution_m is different + if original_resolution_m != target_resolution_m: + zoom_factor = original_resolution_m / target_resolution_m + output_cost_array = scipy.ndimage.zoom( + combined_cost_original_res, + zoom_factor, + order=3, + mode="constant", + cval=np.nan, + ) + self.get_logger().info( + f"Cost map interpolated from {original_resolution_m}m to {target_resolution_m}m resolution." + ) + # If interpolated, the transform needs to be adjusted for the new pixel size + # The top-left corner remains the same, but the resolution changes + adjusted_transform = Affine( + target_resolution_m, + adjusted_transform.b, + adjusted_transform.c, + adjusted_transform.d, + -target_resolution_m, + adjusted_transform.f, + ) + + else: + output_cost_array = combined_cost_original_res + self.get_logger().info( + f"Cost map generated at original {original_resolution_m}m resolution (no interpolation needed)." + ) + + return output_cost_array, adjusted_transform, original_crs + + def _write_elevation_costmap( + self, + cost_array, + costmap_origin_transform, + costmap_crs, + output_filepath_stem, + resolution_m, + ): + """ + Writes the costmap as a grayscale PNG image and a YAML file with metadata. + This function expects the cost_array to be already cropped to valid data extent. + + Args: + cost_array (np.ndarray): The 2D NumPy array of cost values (0.0 to 1.0 or NaN for unknown). + costmap_origin_transform (rasterio.transform.Affine): The affine transform of the *cropped* costmap's top-left. + costmap_crs (rasterio.crs.CRS): The CRS of the costmap. + output_filepath_stem (str): The base path and filename stem (e.g., '/path/to/costmaps/tile_X_Y'). + '.png' and '.yaml' extensions will be added automatically. + resolution_m (float): The resolution of the costmap in meters (after any interpolation). + """ + if cost_array is None or cost_array.size == 0: + self.get_logger().warning("No cost array provided for writing.") + return + + # Ensure output directory exists + output_dir = os.path.dirname(output_filepath_stem) + os.makedirs(output_dir, exist_ok=True) + + # Make a copy to avoid modifying the original array passed in + cost_array_processed = np.copy(cost_array) + + # Define the value for unknown/NaN regions in the output image (255) + UNKNOWN_VALUE_PNG = 255 + + # Set NaN values to the UNKNOWN_VALUE_PNG + cost_array_processed[np.isnan(cost_array_processed)] = ( + UNKNOWN_VALUE_PNG + 1 + ) # Temporarily set to > 255 for scaling + + # Scale cost values (0.0-1.0) to 0-254. Values set to UNKNOWN_VALUE_PNG+1 will map correctly to UNKNOWN_VALUE_PNG. + scaled_cost_array = (cost_array_processed * 254).astype(np.uint8) + + # Explicitly set the values that were originally NaN (or our placeholder) to 255 + scaled_cost_array[scaled_cost_array > 254] = UNKNOWN_VALUE_PNG + + image_filename = f"{output_filepath_stem}.png" + plt.imsave(image_filename, scaled_cost_array, cmap="gray", vmin=0, vmax=255) + self.get_logger().info( + f"Costmap image saved to: {image_filename} (dimensions: {scaled_cost_array.shape[0]}x{scaled_cost_array.shape[1]})" + ) + + # Prepare and save YAML metadata + rows, cols = scaled_cost_array.shape # Use shape of the actual saved image + + minx, maxy = costmap_origin_transform * ( + 0, + 0, + ) # Top-left (col, row) -> (x, y) of the *cropped* image + maxx, miny = costmap_origin_transform * ( + cols, + rows, + ) # Bottom-right (col, row) -> (x, y) of the *cropped* image + + # Create a transformer from costmap_crs to WGS84 (lat/lon) + wgs84_transformer = Transformer.from_crs( + costmap_crs, "EPSG:4326", always_xy=True + ) + + # Convert corner bounds to Lat/Lon + top_left_lon, top_left_lat = wgs84_transformer.transform(minx, maxy) + top_right_lon, top_right_lat = wgs84_transformer.transform(maxx, maxy) + bottom_right_lon, bottom_right_lat = wgs84_transformer.transform(maxx, miny) + bottom_left_lon, bottom_left_lat = wgs84_transformer.transform(minx, miny) + + # Calculate center point in original CRS (UTM) + center_x = (minx + maxx) / 2 + center_y = (miny + maxy) / 2 + center_lon, center_lat = wgs84_transformer.transform(center_x, center_y) + + # --- Calculate Map Heading --- + # The "up" direction of the map corresponds to the vector pointing from a pixel + # to the pixel directly above it in the image (decreasing row index). + # This vector in pixel space is (0, -1). + # Applying the affine transform to this vector gives its direction in world coordinates: + # dx = transform.a * 0 + transform.b * (-1) = -transform.b + # dy = transform.d * 0 + transform.e * (-1) = -transform.e + up_vector_x = -costmap_origin_transform.b + up_vector_y = -costmap_origin_transform.e + + # Calculate the angle of this "up" vector counter-clockwise from the positive Easting (X) axis. + # math.atan2 returns an angle in radians in the range (-pi, pi]. + angle_rad_ccw_from_easting = math.atan2(up_vector_y, up_vector_x) + angle_deg_ccw_from_easting = math.degrees(angle_rad_ccw_from_easting) + + # Normalize the angle to be within [0, 360) + map_north_direction_deg_ccw_from_easting = ( + angle_deg_ccw_from_easting % 360 + 360 + ) % 360 + + # Calculate heading clockwise from North (0 degrees). + # North is +Y, which is 90 degrees CCW from +X (Easting). + # So, angle from North CCW = (angle from Easting CCW) - 90 + # heading clockwise from North = (360 - angle from North CCW) % 360 + angle_from_north_ccw = map_north_direction_deg_ccw_from_easting - 90 + map_heading_degrees_clockwise_from_north = (360 - angle_from_north_ccw) % 360 + + metadata = { + "filename": os.path.basename(image_filename), + "resolution_m": resolution_m, + "value_scale": { + "min": 0, + "max": 254, + "unknown": UNKNOWN_VALUE_PNG, + }, # Added unknown value + "corners_lat_lon": { + "top_left": {"lat": top_left_lat, "lon": top_left_lon}, + "top_right": {"lat": top_right_lat, "lon": top_right_lon}, + "bottom_right": {"lat": bottom_right_lat, "lon": bottom_right_lon}, + "bottom_left": {"lat": bottom_left_lat, "lon": bottom_left_lon}, + }, + "center_lat_lon": {"lat": center_lat, "lon": center_lon}, + # New metadata fields for map orientation/heading + "map_orientation": { + "map_north_direction_deg_ccw_from_easting": map_north_direction_deg_ccw_from_easting, # Angle of the "up" vector + "heading_deg_clockwise_from_north": map_heading_degrees_clockwise_from_north, + }, + } + + yaml_filename = f"{output_filepath_stem}.yaml" + with open(yaml_filename, "w") as f: + yaml.dump(metadata, f, default_flow_style=False) + self.get_logger().info(f"Costmap metadata saved to: {yaml_filename}") + + def download_all_dtms(self): + """ + Downloads DTM files for all specified datasets and generates dtm_index.yaml files. + The output path is structured under self.data_base_dir. + """ + for dataset_name in self.dataset_names: + self.get_logger().info(f"Processing dataset: {dataset_name}") + try: + # Retrieve URL for the current dataset + dataset_url_param_name = f"{dataset_name}_url" + + try: + self.declare_parameter(dataset_url_param_name, "") + except Exception as e: + self.get_logger().debug( + f"Parameter {dataset_url_param_name} already declared. Using existing value. {e}" + ) + + if not self.has_parameter(dataset_url_param_name): + self.get_logger().error( + f"Missing parameter: {dataset_url_param_name}. Skipping dataset {dataset_name}." + ) + continue + + base_data_directory_url = ( + self.get_parameter(dataset_url_param_name) + .get_parameter_value() + .string_value + ) + self.get_logger().info( + f"Using base URL for {dataset_name}: {base_data_directory_url}" + ) + + # Define specific folders for KML and DTM within the dataset folder + dataset_base_folder = os.path.join(self.data_base_dir, dataset_name) + kml_output_folder = os.path.join( + dataset_base_folder, "kml" + ) # KML in dedicated 'kml' folder + dtm_output_folder = os.path.join( + dataset_base_folder, "dtm" + ) # DTMs in dedicated 'dtm' folder + + # Ensure these new folders exist + os.makedirs(kml_output_folder, exist_ok=True) + os.makedirs(dtm_output_folder, exist_ok=True) + + self.get_logger().info( + f"Downloading DTM files for {dataset_name} to {dtm_output_folder}..." + ) + downloaded_filepaths = self._download_dtm_data_from_kml_index( + base_data_directory_url, + kml_output_folder, # Pass the dedicated KML folder + dtm_output_folder, # Pass the dedicated DTM folder + ) + + if downloaded_filepaths: + self.get_logger().info( + f"Successfully downloaded {len(downloaded_filepaths)} DTM files for {dataset_name}." + ) + + # Generate dtm_index.yaml within the dataset_base_folder + dtm_index_path = os.path.join(dataset_base_folder, "dtm_index.yaml") + dtm_info = { + "dataset_name": dataset_name, + "base_url": base_data_directory_url, + "dtm_files": [ + os.path.relpath(fp, dtm_output_folder) + for fp in downloaded_filepaths + ], # Store relative paths + } + with open(dtm_index_path, "w") as f: + yaml.dump(dtm_info, f, default_flow_style=False) + self.get_logger().info( + f"Generated dtm_index.yaml for {dataset_name} at {dtm_index_path}" + ) + else: + self.get_logger().warning( + f"No DTM files downloaded for {dataset_name}." + ) + + except Exception as e: + self.get_logger().error(f"Error processing dataset {dataset_name}: {e}") + + def generate_elevation_costmaps_callback(self, request, response): + """ + ROS2 Service callback to trigger the download of all DTMs and then + generate traversability costmaps for each DTM tile, saving them as PNG and YAML. + """ + self.get_logger().info("Received request to generate elevation costmaps.") + + try: + # 1. Run download_all_dtms (this will also ensure directories exist) + self.download_all_dtms() # This will download only if not already present + + # 2. Iterate over all dataset names + for dataset_name in self.dataset_names: + self.get_logger().info( + f"Generating costmaps for dataset: {dataset_name}" + ) + + dataset_base_folder = os.path.join(self.data_base_dir, dataset_name) + dtm_input_folder = os.path.join( + dataset_base_folder, "dtm" + ) # DTMs are now in 'dtm' subfolder + dtm_index_path = os.path.join( + dataset_base_folder, "dtm_index.yaml" + ) # Index is at dataset root + costmap_output_dir = os.path.join(dataset_base_folder, "costmaps") + os.makedirs( + costmap_output_dir, exist_ok=True + ) # Ensure costmaps directory exists + + if not os.path.exists(dtm_index_path): + self.get_logger().warning( + f"DTM index file not found for {dataset_name} at {dtm_index_path}. Skipping costmap generation for this dataset." + ) + continue + + downloaded_filepaths = [] + try: + with open(dtm_index_path, "r") as f: + dtm_info = yaml.safe_load(f) + if "dtm_files" in dtm_info: + # Construct full paths using the dtm_input_folder + downloaded_filepaths = [ + os.path.join(dtm_input_folder, f) + for f in dtm_info["dtm_files"] + ] + else: + self.get_logger().error( + f"dtm_files key not found in {dtm_index_path}. Skipping." + ) + continue + except Exception as e: + self.get_logger().error( + f"Error loading DTM index file for {dataset_name}: {e}. Skipping." + ) + continue + + if not downloaded_filepaths: + self.get_logger().warning( + f"No DTM files listed in {dtm_index_path}. Skipping costmap generation for this dataset." + ) + continue + + # 3. Iterate over all DTM files in the dataset + for dtm_filepath in downloaded_filepaths: + if not os.path.exists(dtm_filepath): + self.get_logger().warning( + f"DTM file {dtm_filepath} not found. Skipping costmap generation for this tile." + ) + continue + + self.get_logger().info( + f"Processing DTM tile: {os.path.basename(dtm_filepath)}" + ) + try: + with rasterio.open(dtm_filepath) as src: + elevation_array_tile = src.read(1) + original_resolution_m = src.res[ + 0 + ] # Assuming square pixels, res is (width, height) + src_transform = src.transform + src_crs = src.crs + + # Transform all no_data values to NaN for processing + # Get the no_data value from the rasterio source if it exists + no_data_value = src.nodata + if no_data_value is not None: + elevation_array_tile = np.where( + elevation_array_tile == no_data_value, + np.nan, + elevation_array_tile, + ) + + # Print number of NaNs for debugging + num_nans = np.sum(np.isnan(elevation_array_tile)) + self.get_logger().info( + f"Tile {os.path.basename(dtm_filepath)} contains {num_nans} NaN values (no_data)." + ) + + # Call calculate_traversability_cost with ROS2 parameters + ( + traversability_cost_map, + adjusted_transform, + adjusted_crs, + ) = self._calculate_traversability_cost( + elevation_array_tile, + original_resolution_m, + self.target_map_resolution_meters, # Use ROS parameter for target resolution + src_transform, # Pass original transform + src_crs, # Pass original CRS + self.max_chunk_dimension, # Pass ROS parameter for max chunk dimension + slope_weight=self.slope_weight, + roughness_weight=self.roughness_weight, + max_slope_deg=self.max_slope_deg, + max_roughness_m=self.max_roughness_m, + roughness_blur_sigma=self.roughness_blur_sigma, + ) + + if traversability_cost_map is not None: + tile_filename_stem = os.path.splitext( + os.path.basename(dtm_filepath) + )[0] + output_filepath_stem = os.path.join( + costmap_output_dir, tile_filename_stem + ) + + # Pass adjusted_transform and adjusted_crs to write_elevation_costmap + self._write_elevation_costmap( + traversability_cost_map, + adjusted_transform, + adjusted_crs, + output_filepath_stem, + self.target_map_resolution_meters, # Use ROS parameter for resolution + ) + else: + self.get_logger().warning( + f"Failed to calculate traversability cost map for {dtm_filepath}. Possibly all data was NaN." + ) + except rasterio.errors.RasterioIOError as e: + self.get_logger().error( + f"Error reading GeoTIFF file {dtm_filepath}: {e}. Skipping tile." + ) + except Exception as e: + self.get_logger().error( + f"An unexpected error occurred while processing tile {dtm_filepath}: {e}. Skipping tile." + ) + traceback.print_exc() + + response.success = True + response.message = "All elevation costmaps generated successfully." + self.get_logger().info(response.message) + + except Exception as e: + response.success = False + response.message = f"An error occurred during costmap generation: {e}" + self.get_logger().error(response.message) + + return response + + +def main(args=None): + rclpy.init(args=args) + node = CanElevationDownloader() # Renamed node + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/costmap_seeding/costmap_seeding/fake_gps_publisher.py b/src/costmap_seeding/costmap_seeding/fake_gps_publisher.py new file mode 100644 index 00000000..f44dda5f --- /dev/null +++ b/src/costmap_seeding/costmap_seeding/fake_gps_publisher.py @@ -0,0 +1,39 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix + + +class FakeGpsPublisher(Node): + def __init__(self): + super().__init__("fake_gps_publisher") + self.publisher_ = self.create_publisher(NavSatFix, "/gps/fix", 10) + timer_period = 1.0 # seconds (for 1 Hz, adjust for your desired rate) + self.timer = self.create_timer(timer_period, self.publish_gps_fix) + self.get_logger().info("GPS Publisher Node Started") + + def publish_gps_fix(self): + msg = NavSatFix() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "gps_link" + msg.status.status = 2 + msg.status.service = 1 + msg.latitude = 45.385166422 + msg.longitude = -75.69850266 + msg.altitude = 0.0 + msg.position_covariance = [0.001] * 9 + msg.position_covariance_type = 3 + + self.publisher_.publish(msg) + # self.get_logger().info(f'Publishing GPS Fix at {msg.header.stamp.sec}.{msg.header.stamp.nanosec}') + + +def main(args=None): + rclpy.init(args=args) + gps_publisher = FakeGpsPublisher() + rclpy.spin(gps_publisher) + gps_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/costmap_seeding/costmap_seeding/load_can_elevation_maps.py b/src/costmap_seeding/costmap_seeding/load_can_elevation_maps.py new file mode 100644 index 00000000..1e623339 --- /dev/null +++ b/src/costmap_seeding/costmap_seeding/load_can_elevation_maps.py @@ -0,0 +1,807 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + + +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import OccupancyGrid +from geometry_msgs.msg import PoseStamped, Quaternion, Point +from std_srvs.srv import Trigger +from robot_localization.srv import FromLL, ToLL + +import os +import yaml +import cv2 +import numpy as np +import math +import time +from threading import Event + + +class MapLoader(Node): + """ + A ROS2 node that loads and publishes an OccupancyGrid map based on + current GPS location and local map files. It can combine multiple + overlapping maps and fills uncovered areas with unknown values. + """ + + def __init__(self): + super().__init__("map_loader") + + # Declare parameters for map directory, the size of the square region of interest, + # and the GPS covariance threshold. + self.declare_parameter( + "map_directory", "/usr/local/cprt/elevation_datasets/CanElevation/" + ) + self.declare_parameter("map_square_size_m", 2000.0) # Default to 500m square + self.declare_parameter( + "gps_covariance_threshold", 0.5 + ) # New parameter for covariance threshold + + self.map_directory = ( + self.get_parameter("map_directory").get_parameter_value().string_value + ) + self.map_square_size_m = ( + self.get_parameter("map_square_size_m").get_parameter_value().double_value + ) + self.gps_covariance_threshold = ( + self.get_parameter("gps_covariance_threshold") + .get_parameter_value() + .double_value + ) + + self.get_logger().info(f"Initialized MapLoader Node.") + self.get_logger().info(f"Map directory set to: {self.map_directory}") + self.get_logger().info( + f"Map square size for region of interest: {self.map_square_size_m} meters" + ) + self.get_logger().info( + f"GPS covariance threshold set to: {self.gps_covariance_threshold}" + ) + + # Callback groups to prevent deadlocks with multiple subscriptions/services + self.gps_callback_group = MutuallyExclusiveCallbackGroup() + self.service_callback_group = MutuallyExclusiveCallbackGroup() + self.toLL_callback_group = ( + MutuallyExclusiveCallbackGroup() + ) # Dedicated for toLL client + self.fromLL_callback_group = ( + MutuallyExclusiveCallbackGroup() + ) # Dedicated for fromLL client + self.timer_callback_group = MutuallyExclusiveCallbackGroup() + + # GPS Subscriber: Subscribes to /gps/fix to get current location data. + # QoS profile ensures reliable delivery of the latest GPS fix. + qos_profile_transient = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, # Added durability policy + ) + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + ) + self.latest_gps_fix = None # Stores the most recent good GPS fix + self.gps_fix_subscriber = self.create_subscription( + NavSatFix, + "/gps/fix", + self.gps_fix_callback, + qos_profile, + callback_group=self.gps_callback_group, + ) + self.get_logger().info("GPS Fix subscriber created on /gps/fix.") + + # Map Publisher: Publishes the generated OccupancyGrid map. + self.map_publisher = self.create_publisher( + OccupancyGrid, "/blank_map", qos_profile_transient + ) + self.get_logger().info("OccupancyGrid publisher created on /map_can_elevation.") + + # FromLL Service Client: Used to convert Lat/Lon/Alt to map coordinates. + self.fromLL_client = self.create_client( + FromLL, "fromLL", callback_group=self.fromLL_callback_group + ) + # Wait for the fromLL service to be available before proceeding + self.get_logger().info("Waiting for fromLL service to be available...") + while not self.fromLL_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("fromLL service not available, waiting again...") + self.get_logger().info("fromLL service client successfully connected.") + + # ToLL Service Client: Used to convert map coordinates to Lat/Lon/Alt. + self.toLL_client = self.create_client( + ToLL, + "toLL", + callback_group=self.toLL_callback_group, # Using general service group + ) + # Wait for the toLL service to be available before proceeding + self.get_logger().info("Waiting for toLL service to be available...") + while not self.toLL_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("toLL service not available, waiting again...") + self.get_logger().info("toLL service client successfully connected.") + + # Map Generation Service: Allows external triggers for map generation. + self.generate_map_service = self.create_service( + Trigger, + "generate_map_service", + self.load_maps_service_callback, + callback_group=self.service_callback_group, + ) + self.get_logger().info("Generate Map service created on /generate_map_service.") + + # Call load_maps once at startup as required + self.get_logger().info("Triggering initial map loading on startup...") + # Use a timer to call load_maps to ensure ROS2 is fully spun up, + # especially for service calls. This timer will use the default callback group + # or a dedicated one if specified. + self.timer = self.create_timer( + 1.0, + self._initial_map_loading_timer_callback, + callback_group=self.timer_callback_group, + ) + + def _initial_map_loading_timer_callback(self): + """Callback for the one-shot timer to trigger initial map loading.""" + self.destroy_timer(self.timer) # Destroy the timer after its first call + self.get_logger().info("Initial map loading timer triggered.") + self.load_maps() + + def gps_fix_callback(self, msg: NavSatFix): + """ + Callback for the /gps/fix topic. Stores the latest GPS fix if its + position covariance (X and Y components) is below the specified threshold. + Altitude covariance is ignored as per user request. + """ + # Check if covariance type is known or approximated and variances are low + # msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN # This line forces it, remove if not intended + if ( + msg.position_covariance_type == NavSatFix.COVARIANCE_TYPE_KNOWN + or msg.position_covariance_type == NavSatFix.COVARIANCE_TYPE_APPROXIMATED + ): + # NavSatFix.position_covariance is a float64[9] array. + # Indices 0 and 4 correspond to X and Y variances respectively. + # Index 8 corresponds to Z (altitude) variance. + # Only checking X and Y variances as per user request. + if ( + msg.position_covariance[0] < self.gps_covariance_threshold + and msg.position_covariance[4] < self.gps_covariance_threshold + ): + self.latest_gps_fix = msg + self.get_logger().debug( + f"Good GPS fix received: Lat={msg.latitude:.6f}, Lon={msg.longitude:.6f}" + ) + else: + self.get_logger().debug( + f"GPS fix covariance too high (threshold: {self.gps_covariance_threshold:.2f}): x={msg.position_covariance[0]:.2f}, " + f"y={msg.position_covariance[4]:.2f}" + ) + else: + self.get_logger().debug( + "GPS fix covariance type not known or approximated, skipping." + ) + + def load_maps_service_callback( + self, request: Trigger.Request, response: Trigger.Response + ): + """ + Callback for the generate_map_service. Triggers map loading. + """ + self.get_logger().info("Generate Map service called. Triggering map loading...") + self.load_maps() + response.success = True + response.message = "Map loading triggered." + return response + + def convert_lat_lon_to_pose( + self, + latitude: float, + longitude: float, + altitude: float, + orientation: Quaternion, + ) -> PoseStamped: + """ + Converts a latitude, longitude, altitude, and orientation to a PoseStamped + in the map frame using the 'fromLL' service. + This function uses threading.Event to wait for the service response. + """ + req = FromLL.Request() + req.ll_point.longitude = longitude + req.ll_point.latitude = latitude + req.ll_point.altitude = altitude + + self.get_logger().info( + f"Requesting map pose for: Lat={req.ll_point.latitude:.8f}, Long={req.ll_point.longitude:.8f}, Alt={req.ll_point.altitude:.8f}" + ) + + try: + event = Event() + + def done_callback(future): + nonlocal event + event.set() + + future = self.fromLL_client.call_async(req) + future.add_done_callback(done_callback) + + # Wait for the event with a timeout to prevent indefinite hangs + if not event.wait(timeout=5.0): # 5-second timeout + self.get_logger().warn("Timeout waiting for fromLL service response.") + if not rclpy.ok(): + self.get_logger().info( + "ROS2 context is shutting down, aborting lat/lon to map conversion." + ) + return None + + if future.done() and future.exception() is None: + result = future.result() + if result: + target_pose = PoseStamped() + target_pose.header.frame_id = "map" + target_pose.header.stamp = self.get_clock().now().to_msg() + target_pose.pose.position = result.map_point + target_pose.pose.orientation = orientation + self.get_logger().info( + f"Converted to map frame: x={target_pose.pose.position.x:.2f}, y={target_pose.pose.position.y:.2f}" + ) + return target_pose + else: + self.get_logger().error( + "Failed to convert lat/lon to map pose: Service returned no result." + ) + return None + else: + self.get_logger().error( + f"fromLL service call failed: {future.exception()}" + ) + return None + except Exception as e: + self.get_logger().error(f"Error during lat/lon conversion: {e}") + return None + + def convert_pose_to_lat_lon(self, map_point: Point) -> dict: + """ + Converts a Point in the map frame to latitude, longitude, and altitude + using the 'toLL' service. + This function uses threading.Event to wait for the service response, + with a timeout for graceful shutdown. + """ + req = ToLL.Request() + req.map_point = map_point + + self.get_logger().info( + f"Requesting Lat/Lon for map point: x={req.map_point.x:.2f}, y={req.map_point.y:.2f}, z={req.map_point.z:.2f}" + ) + + try: + event = Event() + + def done_callback(future): + nonlocal event + event.set() + + future = self.toLL_client.call_async(req) + future.add_done_callback(done_callback) + + if not event.wait(timeout=5.0): # 5-second timeout + self.get_logger().warn("Timeout waiting for toLL service response.") + if not rclpy.ok(): + self.get_logger().info( + "ROS2 context is shutting down, aborting map to lat/lon conversion." + ) + return None + + if future.done() and future.exception() is None: + result = future.result() + if result: + self.get_logger().info( + f"Converted to Lat/Lon: Lat={result.ll_point.latitude:.8f}, Lon={result.ll_point.longitude:.8f}, Alt={result.ll_point.altitude:.8f}" + ) + return { + "latitude": result.ll_point.latitude, + "longitude": result.ll_point.longitude, + "altitude": result.ll_point.altitude, + } + else: + self.get_logger().error( + "Failed to convert map pose to lat/lon: Service returned no result." + ) + return None + else: + self.get_logger().error( + f"toLL service call failed: {future.exception()}" + ) + return None + except Exception as e: + self.get_logger().error(f"Error during map to lat/lon conversion: {e}") + return None + + def load_maps(self): + self.get_logger().info("Starting optimized map loading process...") + + # Reset latest_gps_fix to ensure we wait for a new, good reading + self.latest_gps_fix = None + self.get_logger().info( + f"Resetting GPS fix and waiting for a new good GPS fix (covariance < {self.gps_covariance_threshold:.2f})..." + ) + + # 1. Wait for a good GPS fix + # Loop until a good GPS fix is received. + while self.latest_gps_fix is None: + self.get_logger().info("No good GPS fix yet, waiting...") + time.sleep(0.5) # Wait for a short duration before checking again + if not rclpy.ok(): # Check if ROS2 context is still valid + self.get_logger().info( + "ROS2 shutdown detected while waiting for GPS. Aborting map loading." + ) + return + + current_gps = self.latest_gps_fix + self.get_logger().info( + f"Using current good GPS fix: Lat={current_gps.latitude:.6f}, Lon={current_gps.longitude:.6f}, Alt={current_gps.altitude:.2f}" + ) + + # A dummy identity quaternion is used as orientation is not relevant for position conversion + dummy_orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + # 2. Define the query square region centered on the current GPS location in Lat/Lon + # First, convert the current GPS location to map coordinates + current_map_pose = self.convert_lat_lon_to_pose( + current_gps.latitude, + current_gps.longitude, + current_gps.altitude, + dummy_orientation, + ) + + if current_map_pose is None: + self.get_logger().error( + "Could not convert current GPS to map coordinates. Aborting map loading." + ) + return + + current_map_x = current_map_pose.pose.position.x + current_map_y = current_map_pose.pose.position.y + + half_square_size_map = self.map_square_size_m / 2.0 + + # Calculate the four corners of the query square in map coordinates + corners_map_frame = [ + Point( + x=current_map_x - half_square_size_map, + y=current_map_y - half_square_size_map, + z=0.0, + ), # Bottom-Left + Point( + x=current_map_x + half_square_size_map, + y=current_map_y - half_square_size_map, + z=0.0, + ), # Bottom-Right + Point( + x=current_map_x - half_square_size_map, + y=current_map_y + half_square_size_map, + z=0.0, + ), # Top-Left + Point( + x=current_map_x + half_square_size_map, + y=current_map_y + half_square_size_map, + z=0.0, + ), # Top-Right + ] + + # Convert these map coordinates back to Lat/Lon using the toLL service + converted_corners_ll = [] + for corner_map_point in corners_map_frame: + ll_point = self.convert_pose_to_lat_lon(corner_map_point) + if ll_point: + converted_corners_ll.append(ll_point) + else: + self.get_logger().error( + f"Failed to convert map corner {corner_map_point} to Lat/Lon. Aborting map loading." + ) + return + + # Determine the min/max Lat/Lon for the query region from the converted corners + query_min_lat = min(c["latitude"] for c in converted_corners_ll) + query_max_lat = max(c["latitude"] for c in converted_corners_ll) + query_min_lon = min(c["longitude"] for c in converted_corners_ll) + query_max_lon = max(c["longitude"] for c in converted_corners_ll) + + self.get_logger().info( + f"Query region in Lat/Lon (via toLL service): Lat[{query_min_lat:.6f}, {query_max_lat:.6f}], Lon[{query_min_lon:.6f}, {query_max_lon:.6f}]" + ) + + # 3. Find all suitable map files that intersect with the query region (Lat/Lon check) + intersecting_maps_data = [] + + for root, dirs, files in os.walk(self.map_directory): + if "costmaps" in dirs: + costmaps_dir = os.path.join(root, "costmaps") + self.get_logger().info( + f"Searching for maps in subdirectory: {costmaps_dir}" + ) + + yaml_files = [ + f for f in os.listdir(costmaps_dir) if f.endswith(".yaml") + ] + self.get_logger().info( + f"Found {len(yaml_files)} YAML files in {costmaps_dir}." + ) + + for yaml_file in yaml_files: + yaml_path = os.path.join(costmaps_dir, yaml_file) + try: + with open(yaml_path, "r") as f: + map_metadata = yaml.safe_load(f) + + # Extract required metadata fields from the YAML file + map_filename = map_metadata.get("filename") + resolution_m = map_metadata.get("resolution_m") + corners_lat_lon = map_metadata.get("corners_lat_lon") + value_scale = map_metadata.get("value_scale") + center_lat_lon = map_metadata.get("center_lat_lon") + + if not all( + [ + map_filename, + resolution_m is not None, + corners_lat_lon, + value_scale, + center_lat_lon, + ] + ): + self.get_logger().warn( + f"Skipping {yaml_file}: Missing one or more required metadata fields." + ) + continue + + png_path = os.path.join( + costmaps_dir, map_filename + ) # PNG is in the same costmaps directory + if not os.path.exists(png_path): + self.get_logger().warn( + f"Skipping {yaml_file}: Corresponding PNG file '{map_filename}' not found at '{png_path}'." + ) + continue + + # Extract map corners directly in Lat/Lon + bl_lat = corners_lat_lon["bottom_left"]["lat"] + bl_lon = corners_lat_lon["bottom_left"]["lon"] + tr_lat = corners_lat_lon["top_right"]["lat"] + tr_lon = corners_lat_lon["top_right"]["lon"] + + # Determine the bounding box of the map in Lat/Lon + map_min_lat = min(bl_lat, tr_lat) + map_max_lat = max(bl_lat, tr_lat) + map_min_lon = min(bl_lon, tr_lon) + map_max_lon = max(bl_lon, tr_lon) + + # Check for intersection between map bounding box (Lat/Lon) and query region (Lat/Lon) + intersects_lat = not ( + map_max_lat < query_min_lat or map_min_lat > query_max_lat + ) + intersects_lon = not ( + map_max_lon < query_min_lon or map_min_lon > query_max_lon + ) + + if intersects_lat and intersects_lon: + self.get_logger().info( + f"Map '{map_filename}' intersects with the current region of interest (Lat/Lon check)." + ) + # If it intersects, convert its corners to map coordinates for later combination + bl_map_pose = self.convert_lat_lon_to_pose( + bl_lat, bl_lon, 0.0, dummy_orientation + ) + tr_map_pose = self.convert_lat_lon_to_pose( + tr_lat, tr_lon, 0.0, dummy_orientation + ) + + if bl_map_pose is None or tr_map_pose is None: + self.get_logger().warn( + f"Skipping {yaml_file}: Could not convert map corners to map coordinates for combination." + ) + continue + + map_min_x = min( + bl_map_pose.pose.position.x, tr_map_pose.pose.position.x + ) + map_max_x = max( + bl_map_pose.pose.position.x, tr_map_pose.pose.position.x + ) + map_min_y = min( + bl_map_pose.pose.position.y, tr_map_pose.pose.position.y + ) + map_max_y = max( + bl_map_pose.pose.position.y, tr_map_pose.pose.position.y + ) + + intersecting_maps_data.append( + { + "png_path": png_path, + "metadata": map_metadata, + "map_min_x": map_min_x, + "map_max_x": map_max_x, + "map_min_y": map_min_y, + "map_max_y": map_max_y, + } + ) + else: + self.get_logger().debug( + f"Map '{map_filename}' in {costmaps_dir} does not intersect (Lat/Lon check). Map bounds: Lat[{map_min_lat:.6f}, {map_max_lat:.6f}], Lon[{map_min_lon:.6f}, {map_max_lon:.6f}]. Query bounds: Lat[{query_min_lat:.6f}, {query_max_lat:.6f}], Lon[{query_min_lon:.6f}, {query_max_lon:.6f}]." + ) + + except yaml.YAMLError as e: + self.get_logger().error( + f"Error parsing YAML file '{yaml_file}' in {costmaps_dir}: {e}" + ) + except Exception as e: + self.get_logger().error( + f"An unexpected error occurred while processing '{yaml_file}' in {costmaps_dir}: {e}" + ) + + if not intersecting_maps_data: + self.get_logger().warn( + "No maps found that intersect with the current GPS location's region of interest. Cannot generate map." + ) + return + + self.get_logger().info("Found the following intersecting maps:") + i = 1 + for map_data in intersecting_maps_data: + self.get_logger().info(f"PNG Path {i}: {map_data['png_path']}") + i += 1 + + # 4. Determine the overall bounding box for the combined map + # The combined map will strictly be map_square_size_m centered on current_map_x, current_map_y + half_square_size_map = self.map_square_size_m / 2.0 + global_min_x = current_map_x - half_square_size_map + global_max_x = current_map_x + half_square_size_map + global_min_y = current_map_y - half_square_size_map + global_max_y = current_map_y + half_square_size_map + + # Define the target resolution for the final OccupancyGrid + target_map_resolution = 0.2 # meters/pixel + + # Get the resolution of the source PNG maps (assuming consistent across all) + # This is assumed to be 1.0 m/pixel based on the user's request for 5x upscaling. + source_map_resolution = intersecting_maps_data[0]["metadata"]["resolution_m"] + + # Calculate dimensions of the combined OccupancyGrid in pixels at the SOURCE resolution + combined_width_m = global_max_x - global_min_x + combined_height_m = global_max_y - global_min_y + + combined_width_pixels_source_res = int( + math.ceil(combined_width_m / source_map_resolution) + ) + combined_height_pixels_source_res = int( + math.ceil(combined_height_m / source_map_resolution) + ) + + self.get_logger().info( + f"Combined map dimensions (source res): {combined_width_pixels_source_res}x{combined_height_pixels_source_res} pixels at {source_map_resolution}m/pixel." + ) + self.get_logger().info( + f"Combined map origin (map frame): X={global_min_x:.2f}, Y={global_min_y:.2f}" + ) + + # Initialize the combined OccupancyGrid data with -1 (unknown) at SOURCE resolution + combined_occupancy_data_source_res = np.full( + (combined_height_pixels_source_res, combined_width_pixels_source_res), + -1, + dtype=np.int8, + ) + + # 5. Load and combine PNG images into the global OccupancyGrid (Optimized with NumPy) + # This part will now work with combined_occupancy_data_source_res + for map_data in intersecting_maps_data: + png_path = map_data["png_path"] + map_metadata = map_data["metadata"] + + image = cv2.imread(png_path, cv2.IMREAD_GRAYSCALE) + if image is None: + self.get_logger().error( + f"Failed to load image from '{png_path}'. Skipping this map for combination." + ) + continue + + height_img, width_img = image.shape + value_min = map_metadata["value_scale"]["min"] + value_max = map_metadata["value_scale"]["max"] + value_unknown = map_metadata["value_scale"]["unknown"] + + self.get_logger().info( + f"Processing map '{map_metadata['filename']}' ({width_img}x{height_img} pixels)." + ) + + # Convert image pixel values (0-255) to OccupancyGrid format (0-100 or -1) + occupancy_values_from_img = np.full(image.shape, -1, dtype=np.int8) + + known_pixels_mask = image != value_unknown + + # Apply scaling only to known pixels + occupancy_values_from_img[known_pixels_mask] = np.round( + (image[known_pixels_mask].astype(float) - value_min) + / (value_max - value_min) + * 100.0 + ).astype(np.int8) + + # Ensure values are clamped between 0 and 100 for known pixels + occupancy_values_from_img[known_pixels_mask] = np.clip( + occupancy_values_from_img[known_pixels_mask], 0, 100 + ) + + # ROS OccupancyGrid has bottom-left origin, OpenCV image has top-left origin. + # Flip the image vertically to align its coordinate system with ROS's. + occupancy_values_from_img_ros_oriented = np.flipud( + occupancy_values_from_img + ) + + # Calculate the bottom-left corner of the current map in the 'map' frame + current_map_bl_x = map_data["map_min_x"] + current_map_bl_y = map_data["map_min_y"] + + # Calculate the pixel offset of this map's bottom-left corner relative + # to the combined map's bottom-left origin (at source resolution). + offset_x_pixels = int( + round((current_map_bl_x - global_min_x) / source_map_resolution) + ) + offset_y_pixels = int( + round((current_map_bl_y - global_min_y) / source_map_resolution) + ) + + self.get_logger().debug( + f"Map '{map_metadata['filename']}' pixel offset: ({offset_x_pixels}, {offset_y_pixels})" + ) + + # Determine the intersection region in pixels for both source (current map) + # and destination (combined map) at SOURCE resolution + + # Destination (combined map) coordinates at source resolution + dest_x_start = max(0, offset_x_pixels) + dest_y_start = max(0, offset_y_pixels) + dest_x_end = min( + combined_width_pixels_source_res, offset_x_pixels + width_img + ) + dest_y_end = min( + combined_height_pixels_source_res, offset_y_pixels + height_img + ) + + # Source (current map) coordinates, adjusted for clipping + src_x_start = max(0, -offset_x_pixels) + src_y_start = max(0, -offset_y_pixels) + src_x_end = min( + width_img, combined_width_pixels_source_res - offset_x_pixels + ) + src_y_end = min( + height_img, combined_height_pixels_source_res - offset_y_pixels + ) + + # Check if there's any valid intersection after clipping + if dest_x_end <= dest_x_start or dest_y_end <= dest_y_start: + self.get_logger().debug( + f"Map '{map_metadata['filename']}' has no valid intersection with combined map grid after clipping. Skipping." + ) + continue + + # Get the slices for the destination and source regions + dest_slice = ( + slice(dest_y_start, dest_y_end), + slice(dest_x_start, dest_x_end), + ) + src_slice = (slice(src_y_start, src_y_end), slice(src_x_start, src_x_end)) + + # Extract the relevant parts for processing + current_combined_region = combined_occupancy_data_source_res[dest_slice] + new_map_region = occupancy_values_from_img_ros_oriented[src_slice] + + # Apply max cost logic using NumPy operations + # Mask for pixels where the new map has a known value + new_known_mask = new_map_region != -1 + # Mask for pixels where the combined map currently has an unknown value + current_unknown_mask = current_combined_region == -1 + + # Case 1: New map has a known value, and current combined is unknown -> take new value + combined_occupancy_data_source_res[dest_slice][ + new_known_mask & current_unknown_mask + ] = new_map_region[new_known_mask & current_unknown_mask] + + # Case 2: Both new map and current combined have known values -> take maximum + both_known_mask = ( + new_known_mask & ~current_unknown_mask + ) # ~current_unknown_mask means current is known + combined_occupancy_data_source_res[dest_slice][both_known_mask] = ( + np.maximum( + current_combined_region[both_known_mask], + new_map_region[both_known_mask], + ) + ) + # Other cases (new unknown, current known; both unknown) are implicitly handled: + # - If new is unknown, and current is known, current value is kept. + # - If both are unknown, value remains -1. + + # --- NEW CODE FOR UPSAMPLING --- + # Calculate the scaling factor based on source and target resolutions + scaling_factor = source_map_resolution / target_map_resolution + if not scaling_factor.is_integer(): + self.get_logger().warn( + f"Scaling factor is not an integer ({scaling_factor}). This might lead to unexpected resizing behavior for occupancy grids." + ) + scaling_factor = int( + scaling_factor + ) # Ensure it's an integer for pixel operations + + if scaling_factor > 1: + self.get_logger().info( + f"Upsampling combined map from {source_map_resolution}m/pixel to {target_map_resolution}m/pixel (factor: {scaling_factor}x)." + ) + # Resize the combined map to the target resolution + # Use INTER_NEAREST to preserve discrete occupancy values (0-100, -1) + combined_occupancy_data_target_res = cv2.resize( + combined_occupancy_data_source_res, + ( + combined_width_pixels_source_res * scaling_factor, + combined_height_pixels_source_res * scaling_factor, + ), + interpolation=cv2.INTER_NEAREST, + ) + # Ensure the data type remains int8 after resize + combined_occupancy_data = combined_occupancy_data_target_res.astype(np.int8) + else: + self.get_logger().info("No upsampling needed (scaling factor <= 1).") + combined_occupancy_data = combined_occupancy_data_source_res + + # Update final dimensions for the OccupancyGrid message + combined_height_pixels, combined_width_pixels = combined_occupancy_data.shape + + self.get_logger().info( + f"Final combined map dimensions (target res): {combined_width_pixels}x{combined_height_pixels} pixels at {target_map_resolution}m/pixel." + ) + + # Flatten the NumPy array to a list for OccupancyGrid message + final_occupancy_data_list = combined_occupancy_data.flatten().tolist() + + # 6. Create and populate the OccupancyGrid message + occupancy_grid_msg = OccupancyGrid() + occupancy_grid_msg.header.frame_id = "map" # Map frame + occupancy_grid_msg.header.stamp = self.get_clock().now().to_msg() + + occupancy_grid_msg.info.resolution = float( + target_map_resolution + ) # Use target resolution + occupancy_grid_msg.info.width = combined_width_pixels + occupancy_grid_msg.info.height = combined_height_pixels + + # Set the origin of the combined OccupancyGrid + occupancy_grid_msg.info.origin.position.x = global_min_x + occupancy_grid_msg.info.origin.position.y = global_min_y + occupancy_grid_msg.info.origin.position.z = 0.0 # Assuming a 2D map + # Assuming the map is axis-aligned with the 'map' frame, so orientation is identity + dummy_orientation = Quaternion( + x=0.0, y=0.0, z=0.0, w=1.0 + ) # Re-define if not in scope + occupancy_grid_msg.info.origin.orientation = dummy_orientation + + occupancy_grid_msg.data = final_occupancy_data_list + + # 7. Publish the OccupancyGrid map + self.map_publisher.publish(occupancy_grid_msg) + self.get_logger().info(f"Successfully published combined OccupancyGrid map.") + + +def main(args=None): + rclpy.init(args=args) + node = MapLoader() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/costmap_seeding/launch/download_can_elevation.launch.py b/src/costmap_seeding/launch/download_can_elevation.launch.py new file mode 100644 index 00000000..fb4a8db4 --- /dev/null +++ b/src/costmap_seeding/launch/download_can_elevation.launch.py @@ -0,0 +1,37 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution + + +def generate_launch_description(): + # Get the share directory of your package + costmap_seeding_share_dir = get_package_share_directory("costmap_seeding") + + # Define the path to the parameters YAML file + # We assume the YAML file is located in a 'config' subdirectory within your package's share directory + param_file_path = os.path.join( + costmap_seeding_share_dir, "config", "can_elevation_downloader_params.yaml" + ) + + # Declare a launch argument for the parameters file, allowing it to be overridden + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=TextSubstitution(text=param_file_path), + description="Path to the YAML file with node parameters", + ) + + # Define the node + can_elevation_downloader_node = Node( + package="costmap_seeding", + executable="DownloadCanElevationMaps", # This should match the entry point in setup.py + name="can_elevation_downloader_node", # This should match the node name in the C++ or Python code + parameters=[ + LaunchConfiguration("params_file") + ], # Load parameters from the specified YAML file + ) + + # Create the launch description and add the actions + return LaunchDescription([declare_params_file_cmd, can_elevation_downloader_node]) diff --git a/src/costmap_seeding/package.xml b/src/costmap_seeding/package.xml new file mode 100644 index 00000000..e2ac4afe --- /dev/null +++ b/src/costmap_seeding/package.xml @@ -0,0 +1,18 @@ + + + + costmap_seeding + 0.0.0 + TODO: Package description + erik + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/costmap_seeding/resource/costmap_seeding b/src/costmap_seeding/resource/costmap_seeding new file mode 100644 index 00000000..e69de29b diff --git a/src/costmap_seeding/setup.cfg b/src/costmap_seeding/setup.cfg new file mode 100644 index 00000000..6bc5316d --- /dev/null +++ b/src/costmap_seeding/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/costmap_seeding +[install] +install_scripts=$base/lib/costmap_seeding diff --git a/src/costmap_seeding/setup.py b/src/costmap_seeding/setup.py new file mode 100644 index 00000000..42e6036a --- /dev/null +++ b/src/costmap_seeding/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup +import glob, os + +package_name = "costmap_seeding" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "config"), glob.glob("config/*.yaml")), + (os.path.join("share", package_name, "launch"), glob.glob("launch/*")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="erik", + maintainer_email="erikcaell@gmail.com", + description="Seed the nav2 costmap with elevation data from a online elevation datasets", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "DownloadCanElevationMaps = costmap_seeding.download_can_elevation_maps:main", + "LoadCanElevationMaps = costmap_seeding.load_can_elevation_maps:main", + "FakeGpsPublisher = costmap_seeding.fake_gps_publisher:main", + ], + }, +) diff --git a/src/costmap_seeding/test/test_copyright.py b/src/costmap_seeding/test/test_copyright.py new file mode 100644 index 00000000..ceffe896 --- /dev/null +++ b/src/costmap_seeding/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/src/costmap_seeding/test/test_flake8.py b/src/costmap_seeding/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/src/costmap_seeding/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/src/costmap_seeding/test/test_pep257.py b/src/costmap_seeding/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/src/costmap_seeding/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings"