From 084405f31b5448c78e61e53c928856c6aef823a5 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 17 Jan 2024 15:49:12 +0100 Subject: [PATCH 01/11] Adding max filter and square filter --- .../plugins/max_layer_filter.py | 4 + .../plugins/sqrt_filter.py | 122 ++++++++++++++++++ 2 files changed, 126 insertions(+) create mode 100644 elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py index 63c6a496..f6b2fc46 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py @@ -29,6 +29,7 @@ def __init__( min_or_max: str = "max", thresholds: list = [False], default_value: float = 0.0, + apply_sqrt: bool = True, **kwargs, ): super().__init__() @@ -37,6 +38,7 @@ def __init__( self.min_or_max = min_or_max self.thresholds = thresholds self.default_value = default_value + self.apply_sqrt = apply_sqrt def get_layer_data( self, @@ -119,4 +121,6 @@ def __call__( result = cp.min(result, axis=0) else: result = cp.max(result, axis=0) + if self.apply_sqrt: + result = cp.square(result) return result diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py new file mode 100644 index 00000000..63c6a496 --- /dev/null +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py @@ -0,0 +1,122 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +from typing import List + +from elevation_mapping_cupy.plugins.plugin_manager import PluginBase + + +class MaxLayerFilter(PluginBase): + """Applies a maximum filter to the input layers and updates the traversability map. + This can be used to enhance navigation by identifying traversable areas. + + Args: + cell_n (int): The width and height of the elevation map. + layers (list): List of layers for semantic traversability. Default is ["traversability"]. + thresholds (list): List of thresholds for each layer. Default is [0.5]. + type (list): List of types for each layer. Default is ["traversability"]. + **kwargs: Additional keyword arguments. + """ + + def __init__( + self, + cell_n: int = 100, + layers: list = ["traversability"], + reverse: list = [True], + min_or_max: str = "max", + thresholds: list = [False], + default_value: float = 0.0, + **kwargs, + ): + super().__init__() + self.layers = layers + self.reverse = reverse + self.min_or_max = min_or_max + self.thresholds = thresholds + self.default_value = default_value + + def get_layer_data( + self, + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + name, + ): + if name in layer_names: + idx = layer_names.index(name) + layer = elevation_map[idx].copy() + elif name in plugin_layer_names: + idx = plugin_layer_names.index(name) + layer = plugin_layers[idx].copy() + elif name in semantic_layer_names: + idx = semantic_layer_names.index(name) + layer = semantic_map[idx].copy() + else: + print(f"Could not find layer {name}!") + layer = None + return layer + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + layers = [] + for it, name in enumerate(self.layers): + layer = self.get_layer_data( + elevation_map, layer_names, plugin_layers, plugin_layer_names, semantic_map, semantic_layer_names, name + ) + if layer is None: + continue + if isinstance(self.default_value, float): + layer = cp.where(layer == 0.0, float(self.default_value), layer) + elif isinstance(self.default_value, str): + default_layer = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + self.default_value, + ) + layer = cp.where(layer == 0, default_layer, layer) + if self.reverse[it]: + layer = 1.0 - layer + if isinstance(self.thresholds[it], float): + layer = cp.where(layer > float(self.thresholds[it]), 1, 0) + layers.append(layer) + if len(layers) == 0: + print("No layers are found, returning traversability!") + idx = layer_names.index("traversability") + return elevation_map[idx] + result = cp.stack(layers, axis=0) + if self.min_or_max == "min": + result = cp.min(result, axis=0) + else: + result = cp.max(result, axis=0) + return result From 99bf682f666413be708e66e6c2ee9c0d33cb8388 Mon Sep 17 00:00:00 2001 From: Sergei Sergienko Date: Thu, 1 Feb 2024 11:09:32 +0300 Subject: [PATCH 02/11] fix: Updated included launch name in plane decomposition launch --- README.md | 13 +++++++------ .../turtlesim_plane_decomposition_example.launch | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index e7321e72..975fba85 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,9 @@ [Documentation](https://leggedrobotics.github.io/elevation_mapping_cupy/) - ## Overview -The Elevaton Mapping CuPy software package represents an advancement in robotic navigation and locomotion. +The Elevaton Mapping CuPy software package represents an advancement in robotic navigation and locomotion. Integrating with the Robot Operating System (ROS) and utilizing GPU acceleration, this framework enhances point cloud registration and ray casting, crucial for efficient and accurate robotic movement, particularly in legged robots. ![screenshot](docs/media/main_repo.png) @@ -38,6 +37,7 @@ layers of the map. Finally the map can be post-processed with various custom plu external components (e.g. line detection). ## Citing + If you use the Elevation Mapping CuPy, please cite the following paper: Elevation Mapping for Locomotion and Navigation using GPU @@ -71,7 +71,7 @@ Gian Erni, Jonas Frey, Takahiro Miki, Matias Mattamala, Marco Hutter } ``` -## Quick instructions to run: +## Quick instructions to run ### Installation @@ -87,19 +87,19 @@ Then install dependencies. You can also use docker which already install all dependencies. When you run the script it should pull the image. - ```zsh cd docker ./run.sh ``` -You can also build locally by running `build.sh`. +You can also build locally by running `build.sh`, but in this case change `IMAGE_NAME` in `run.sh` to `elevation_mapping_cupy:latest`. For more information, check [Document](https://leggedrobotics.github.io/elevation_mapping_cupy/getting_started/installation.html) ### Build package Inside docker container. + ```zsh cd $HOME/catkin_ws catkin build elevation_mapping_cupy @@ -107,7 +107,8 @@ catkin build convex_plane_decomposition_ros # If you want to use plane segmenta catkin build semantic_sensor # If you want to use semantic sensors ``` -### Run turtlebot example. +### Run turtlebot example + ![Elevation map examples](docs/media/turtlebot.png) ```bash diff --git a/elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch b/elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch index 0cbd561b..09bfb7c9 100644 --- a/elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch +++ b/elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch @@ -3,7 +3,7 @@ - + From cfb3fc76bd52546acb9ac5e74b001b379a8571df Mon Sep 17 00:00:00 2001 From: Takahiro Date: Sat, 24 Feb 2024 19:36:21 +0100 Subject: [PATCH 03/11] Moved get_layer_data to plugin manager base. Created erosion filter. --- .../config/core/plugin_config.yaml | 12 +- .../anymal/anymal_sensor_parameter.yaml | 2 +- .../elevation_mapping_cupy/plugins/erosion.py | 89 +++++++++++++ .../plugins/max_layer_filter.py | 55 ++++---- .../plugins/plugin_manager.py | 60 ++++++++- .../plugins/sqrt_filter.py | 122 ------------------ 6 files changed, 184 insertions(+), 156 deletions(-) create mode 100644 elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py delete mode 100644 elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py diff --git a/elevation_mapping_cupy/config/core/plugin_config.yaml b/elevation_mapping_cupy/config/core/plugin_config.yaml index 7eb62c4c..efb29b27 100644 --- a/elevation_mapping_cupy/config/core/plugin_config.yaml +++ b/elevation_mapping_cupy/config/core/plugin_config.yaml @@ -25,4 +25,14 @@ inpainting: layer_name: "inpaint" extra_params: method: "telea" # telea or ns -# Apply smoothing for inpainted layer \ No newline at end of file +# Apply smoothing for inpainted layer +erosion: + enable: True + fill_nan: False + is_height_layer: False + layer_name: "erosion" + extra_params: + input_layer_name: "traversability" + dilation_size: 3 + iteration_n: 20 + reverse: True \ No newline at end of file diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml index a130607f..7f5bec6b 100644 --- a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml +++ b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml @@ -23,7 +23,7 @@ publishers: fps: 5.0 filtered_elevation_map: - layers: ['inpaint', 'smooth', 'min_filter'] + layers: ['inpaint', 'smooth', 'min_filter', 'erosion'] basic_layers: ['inpaint'] fps: 5.0 diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py new file mode 100644 index 00000000..b78497b4 --- /dev/null +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py @@ -0,0 +1,89 @@ +# +# Copyright (c) 2024, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cv2 as cv +import cupy as cp +import numpy as np + +from typing import List + +from .plugin_manager import PluginBase + + +class Erosion(PluginBase): + """ + This class is used for applying erosion to an elevation map or specific layers within it. + Erosion is a morphological operation that is used to remove small-scale details from a binary image. + + Args: + kernel_size (int): Size of the erosion kernel. Default is 3, which means a 3x3 square kernel. + iterations (int): Number of times erosion is applied. Default is 1. + **kwargs (): Additional keyword arguments. + """ + + def __init__( + self, + input_layer_name="traversability", + kernel_size: int = 3, + iterations: int = 1, + reverse: bool = False, + **kwargs, + ): + super().__init__() + self.input_layer_name = input_layer_name + self.kernel_size = kernel_size + self.iterations = iterations + self.reverse = reverse + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + Applies erosion to the given elevation map. + + Args: + elevation_map (cupy._core.core.ndarray): The elevation map to be eroded. + layer_names (List[str]): Names of the layers in the elevation map. + plugin_layers (cupy._core.core.ndarray): Layers provided by other plugins. + plugin_layer_names (List[str]): Names of the layers provided by other plugins. + *args (): Additional arguments. + + Returns: + cupy._core.core.ndarray: The eroded elevation map. + """ + # Convert the elevation map to a format suitable for erosion (if necessary) + layer_data = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + self.input_layer_name, + ) + layer_np = cp.asnumpy(layer_data) + + # Define the erosion kernel + kernel = np.ones((self.kernel_size, self.kernel_size), np.uint8) + + if self.reverse: + layer_np = 1 - layer_np + # Apply erosion + layer_min = float(layer_np.min()) + layer_max = float(layer_np.max()) + layer_np_normalized = ((layer_np - layer_min) * 255 / (layer_max - layer_min)).astype("uint8") + eroded_map_np = cv.erode(layer_np_normalized, kernel, iterations=self.iterations) + eroded_map_np = eroded_map_np.astype(np.float32) * (layer_max - layer_min) / 255 + layer_min + if self.reverse: + eroded_map_np = 1 - eroded_map_np + + # Convert back to cupy array and return + return cp.asarray(eroded_map_np) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py index f6b2fc46..3b3dc3c6 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py @@ -15,9 +15,10 @@ class MaxLayerFilter(PluginBase): Args: cell_n (int): The width and height of the elevation map. + reverse (list): A list of boolean values indicating whether to reverse the filter operation for each layer. Default is [True]. + min_or_max (str): A string indicating whether to apply a minimum or maximum filter. Accepts "min" or "max". Default is "max". layers (list): List of layers for semantic traversability. Default is ["traversability"]. - thresholds (list): List of thresholds for each layer. Default is [0.5]. - type (list): List of types for each layer. Default is ["traversability"]. + thresholds (list): List of thresholds for each layer. If the value is bigger than a threshold, assign 1.0 otherwise 0.0. If it is False, it does not apply. Default is [False]. **kwargs: Additional keyword arguments. """ @@ -29,7 +30,6 @@ def __init__( min_or_max: str = "max", thresholds: list = [False], default_value: float = 0.0, - apply_sqrt: bool = True, **kwargs, ): super().__init__() @@ -38,31 +38,30 @@ def __init__( self.min_or_max = min_or_max self.thresholds = thresholds self.default_value = default_value - self.apply_sqrt = apply_sqrt - def get_layer_data( - self, - elevation_map, - layer_names, - plugin_layers, - plugin_layer_names, - semantic_map, - semantic_layer_names, - name, - ): - if name in layer_names: - idx = layer_names.index(name) - layer = elevation_map[idx].copy() - elif name in plugin_layer_names: - idx = plugin_layer_names.index(name) - layer = plugin_layers[idx].copy() - elif name in semantic_layer_names: - idx = semantic_layer_names.index(name) - layer = semantic_map[idx].copy() - else: - print(f"Could not find layer {name}!") - layer = None - return layer + # def get_layer_data( + # self, + # elevation_map, + # layer_names, + # plugin_layers, + # plugin_layer_names, + # semantic_map, + # semantic_layer_names, + # name, + # ): + # if name in layer_names: + # idx = layer_names.index(name) + # layer = elevation_map[idx].copy() + # elif name in plugin_layer_names: + # idx = plugin_layer_names.index(name) + # layer = plugin_layers[idx].copy() + # elif name in semantic_layer_names: + # idx = semantic_layer_names.index(name) + # layer = semantic_map[idx].copy() + # else: + # print(f"Could not find layer {name}!") + # layer = None + # return layer def __call__( self, @@ -121,6 +120,4 @@ def __call__( result = cp.min(result, axis=0) else: result = cp.max(result, axis=0) - if self.apply_sqrt: - result = cp.square(result) return result diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py index 8ff9fda1..26f81d21 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py @@ -4,7 +4,7 @@ # from abc import ABC import cupy as cp -from typing import List, Dict +from typing import List, Dict, Optional import importlib import inspect from dataclasses import dataclass @@ -39,6 +39,8 @@ def __call__( layer_names: List[str], plugin_layers: cp.ndarray, plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], *args, **kwargs, ) -> cp.ndarray: @@ -50,6 +52,8 @@ def __call__( layer_names (): plugin_layers (): plugin_layer_names (): + semantic_map (): + semantic_layer_names (): Run your processing here and return the result. layer of elevation_map 0: elevation @@ -64,6 +68,45 @@ def __call__( """ pass + def get_layer_data( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + name: str, + ) -> Optional[cp.ndarray]: + """ + Retrieve a copy of the layer data from the elevation, plugin, or semantic maps based on the layer name. + + Args: + elevation_map (cp.ndarray): The elevation map containing various layers. + layer_names (List[str]): A list of names for each layer in the elevation map. + plugin_layers (cp.ndarray): The plugin layers containing additional data. + plugin_layer_names (List[str]): A list of names for each layer in the plugin layers. + semantic_map (cp.ndarray): The semantic map containing various layers. + semantic_layer_names (List[str]): A list of names for each layer in the semantic map. + name (str): The name of the layer to retrieve. + + Returns: + Optional[cp.ndarray]: A copy of the requested layer as a cupy ndarray if found, otherwise None. + """ + if name in layer_names: + idx = layer_names.index(name) + layer = elevation_map[idx].copy() + elif name in plugin_layer_names: + idx = plugin_layer_names.index(name) + layer = plugin_layers[idx].copy() + elif name in semantic_layer_names: + idx = semantic_layer_names.index(name) + layer = semantic_map[idx].copy() + else: + print(f"Could not find layer {name}!") + layer = None + return layer + class PluginManager(object): """ @@ -152,11 +195,22 @@ def update_with_name( self.layers[idx] = self.plugins[idx](elevation_map, layer_names, self.layers, self.layer_names) elif n_param == 7: self.layers[idx] = self.plugins[idx]( - elevation_map, layer_names, self.layers, self.layer_names, semantic_map, semantic_params, + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, ) elif n_param == 8: self.layers[idx] = self.plugins[idx]( - elevation_map, layer_names, self.layers, self.layer_names, semantic_map, semantic_params, rotation, + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + rotation, ) else: self.layers[idx] = self.plugins[idx]( diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py deleted file mode 100644 index 63c6a496..00000000 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/sqrt_filter.py +++ /dev/null @@ -1,122 +0,0 @@ -# -# Copyright (c) 2022, Takahiro Miki. All rights reserved. -# Licensed under the MIT license. See LICENSE file in the project root for details. -# -import cupy as cp -import numpy as np -from typing import List - -from elevation_mapping_cupy.plugins.plugin_manager import PluginBase - - -class MaxLayerFilter(PluginBase): - """Applies a maximum filter to the input layers and updates the traversability map. - This can be used to enhance navigation by identifying traversable areas. - - Args: - cell_n (int): The width and height of the elevation map. - layers (list): List of layers for semantic traversability. Default is ["traversability"]. - thresholds (list): List of thresholds for each layer. Default is [0.5]. - type (list): List of types for each layer. Default is ["traversability"]. - **kwargs: Additional keyword arguments. - """ - - def __init__( - self, - cell_n: int = 100, - layers: list = ["traversability"], - reverse: list = [True], - min_or_max: str = "max", - thresholds: list = [False], - default_value: float = 0.0, - **kwargs, - ): - super().__init__() - self.layers = layers - self.reverse = reverse - self.min_or_max = min_or_max - self.thresholds = thresholds - self.default_value = default_value - - def get_layer_data( - self, - elevation_map, - layer_names, - plugin_layers, - plugin_layer_names, - semantic_map, - semantic_layer_names, - name, - ): - if name in layer_names: - idx = layer_names.index(name) - layer = elevation_map[idx].copy() - elif name in plugin_layer_names: - idx = plugin_layer_names.index(name) - layer = plugin_layers[idx].copy() - elif name in semantic_layer_names: - idx = semantic_layer_names.index(name) - layer = semantic_map[idx].copy() - else: - print(f"Could not find layer {name}!") - layer = None - return layer - - def __call__( - self, - elevation_map: cp.ndarray, - layer_names: List[str], - plugin_layers: cp.ndarray, - plugin_layer_names: List[str], - semantic_map: cp.ndarray, - semantic_layer_names: List[str], - *args, - ) -> cp.ndarray: - """ - - Args: - elevation_map (cupy._core.core.ndarray): - layer_names (List[str]): - plugin_layers (cupy._core.core.ndarray): - plugin_layer_names (List[str]): - semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): - *args (): - - Returns: - cupy._core.core.ndarray: - """ - layers = [] - for it, name in enumerate(self.layers): - layer = self.get_layer_data( - elevation_map, layer_names, plugin_layers, plugin_layer_names, semantic_map, semantic_layer_names, name - ) - if layer is None: - continue - if isinstance(self.default_value, float): - layer = cp.where(layer == 0.0, float(self.default_value), layer) - elif isinstance(self.default_value, str): - default_layer = self.get_layer_data( - elevation_map, - layer_names, - plugin_layers, - plugin_layer_names, - semantic_map, - semantic_layer_names, - self.default_value, - ) - layer = cp.where(layer == 0, default_layer, layer) - if self.reverse[it]: - layer = 1.0 - layer - if isinstance(self.thresholds[it], float): - layer = cp.where(layer > float(self.thresholds[it]), 1, 0) - layers.append(layer) - if len(layers) == 0: - print("No layers are found, returning traversability!") - idx = layer_names.index("traversability") - return elevation_map[idx] - result = cp.stack(layers, axis=0) - if self.min_or_max == "min": - result = cp.min(result, axis=0) - else: - result = cp.max(result, axis=0) - return result From 1b304358d163977dee33fd13dfe1295d60ec8c5b Mon Sep 17 00:00:00 2001 From: Takahiro Date: Sat, 24 Feb 2024 19:42:38 +0100 Subject: [PATCH 04/11] Removed commented out code. --- .../plugins/max_layer_filter.py | 24 ------------------- 1 file changed, 24 deletions(-) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py index 3b3dc3c6..1c4e7853 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py @@ -39,30 +39,6 @@ def __init__( self.thresholds = thresholds self.default_value = default_value - # def get_layer_data( - # self, - # elevation_map, - # layer_names, - # plugin_layers, - # plugin_layer_names, - # semantic_map, - # semantic_layer_names, - # name, - # ): - # if name in layer_names: - # idx = layer_names.index(name) - # layer = elevation_map[idx].copy() - # elif name in plugin_layer_names: - # idx = plugin_layer_names.index(name) - # layer = plugin_layers[idx].copy() - # elif name in semantic_layer_names: - # idx = semantic_layer_names.index(name) - # layer = semantic_map[idx].copy() - # else: - # print(f"Could not find layer {name}!") - # layer = None - # return layer - def __call__( self, elevation_map: cp.ndarray, From 6b25b7ede8106c5d4981d7b69a5284fc758a44df Mon Sep 17 00:00:00 2001 From: Takahiro Miki Date: Wed, 28 Feb 2024 17:47:36 +0100 Subject: [PATCH 05/11] Updated bibtex. --- README.md | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 975fba85..057424df 100644 --- a/README.md +++ b/README.md @@ -46,13 +46,13 @@ Elevation Mapping for Locomotion and Navigation using GPU Takahiro Miki, Lorenz Wellhausen, Ruben Grandia, Fabian Jenelten, Timon Homberger, Marco Hutter ```bibtex -@misc{mikielevation2022, - doi = {10.48550/ARXIV.2204.12876}, - author = {Miki, Takahiro and Wellhausen, Lorenz and Grandia, Ruben and Jenelten, Fabian and Homberger, Timon and Hutter, Marco}, - keywords = {Robotics (cs.RO), FOS: Computer and information sciences, FOS: Computer and information sciences}, - title = {Elevation Mapping for Locomotion and Navigation using GPU}, - publisher = {International Conference on Intelligent Robots and Systems (IROS)}, - year = {2022}, +@inproceedings{miki2022elevation, + title={Elevation mapping for locomotion and navigation using gpu}, + author={Miki, Takahiro and Wellhausen, Lorenz and Grandia, Ruben and Jenelten, Fabian and Homberger, Timon and Hutter, Marco}, + booktitle={2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pages={2273--2280}, + year={2022}, + organization={IEEE} } ``` @@ -63,11 +63,13 @@ If you use the Multi-modal Elevation Mapping for color or semantic layers, pleas Gian Erni, Jonas Frey, Takahiro Miki, Matias Mattamala, Marco Hutter ```bibtex -@misc{Erni2023-bs, - title = "{MEM}: {Multi-Modal} Elevation Mapping for Robotics and Learning", - author = "Erni, Gian and Frey, Jonas and Miki, Takahiro and Mattamala, Matias and Hutter, Marco", - publisher = {International Conference on Intelligent Robots and Systems (IROS)}, - year = {2023}, +@inproceedings{erni2023mem, + title={MEM: Multi-Modal Elevation Mapping for Robotics and Learning}, + author={Erni, Gian and Frey, Jonas and Miki, Takahiro and Mattamala, Matias and Hutter, Marco}, + booktitle={2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pages={11011--11018}, + year={2023}, + organization={IEEE} } ``` From d33feca2ad24429d1baa9a0790b5b55c071705e1 Mon Sep 17 00:00:00 2001 From: Takahiro Date: Mon, 25 Mar 2024 21:48:44 +0100 Subject: [PATCH 06/11] Added a param called alwaysClearWithInitializer. --- .../include/elevation_mapping_cupy/elevation_mapping_ros.hpp | 1 + elevation_mapping_cupy/src/elevation_mapping_ros.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index 0149ffad..0fb75b6a 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -184,6 +184,7 @@ class ElevationMappingNode { bool enableDriftCorrectedTFPublishing_; bool useInitializerAtStart_; double initializeTfGridSize_; + bool alwaysClearWithInitializer_; std::atomic_int pointCloudProcessCounter_; }; diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 4a72e8c6..715afeb9 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -68,6 +68,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) nh.param("enable_normal_arrow_publishing", enableNormalArrowPublishing_, false); nh.param("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_, false); nh.param("use_initializer_at_start", useInitializerAtStart_, false); + nh.param("always_clear_with_initializer", alwaysClearWithInitializer_, false); enablePointCloudPublishing_ = enablePointCloudPublishing; @@ -541,6 +542,9 @@ bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request bool ElevationMappingNode::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO("Clearing map."); map_.clear(); + if (alwaysClearWithInitializer_) { + initializeWithTF(); + } return true; } From ee9a15521bb8d270cb1c2e43c7e89ba71e0e5f3c Mon Sep 17 00:00:00 2001 From: Takahiro Date: Mon, 1 Apr 2024 22:41:03 +0200 Subject: [PATCH 07/11] Fixed issue with erosion filter. --- .gitignore | 3 ++- elevation_mapping_cupy/compile_commands.json | 1 - .../elevation_mapping_cupy/plugins/erosion.py | 24 +++++++++++++++++++ 3 files changed, 26 insertions(+), 2 deletions(-) delete mode 120000 elevation_mapping_cupy/compile_commands.json diff --git a/.gitignore b/.gitignore index 69ac53f4..41fde99f 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,5 @@ docs/build _build .idea* .vscode* -*.egg-info \ No newline at end of file +*.egg-info +elevation_mapping_cupy/compile_commands.json diff --git a/elevation_mapping_cupy/compile_commands.json b/elevation_mapping_cupy/compile_commands.json deleted file mode 120000 index 2fdee343..00000000 --- a/elevation_mapping_cupy/compile_commands.json +++ /dev/null @@ -1 +0,0 @@ -/home/takahiro/catkin_ws/build/elevation_mapping_cupy/compile_commands.json \ No newline at end of file diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py index b78497b4..7b0211df 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py @@ -28,6 +28,7 @@ def __init__( kernel_size: int = 3, iterations: int = 1, reverse: bool = False, + default_layer_name: str = "traversability", **kwargs, ): super().__init__() @@ -35,6 +36,7 @@ def __init__( self.kernel_size = kernel_size self.iterations = iterations self.reverse = reverse + self.default_layer_name = default_layer_name def __call__( self, @@ -69,6 +71,28 @@ def __call__( semantic_layer_names, self.input_layer_name, ) + if layer_data is None: + print(f"No layers are found, using {self.default_layer_name}!") + layer_data = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + self.default_layer_name, + ) + if layer_data is None: + print(f"No layers are found, using traversability!") + layer_data = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + "traversability", + ) layer_np = cp.asnumpy(layer_data) # Define the erosion kernel From 91d2a3de18b7c13b53c9581f4f9982948dd95bef Mon Sep 17 00:00:00 2001 From: yyagin Date: Fri, 5 Apr 2024 14:50:17 +0000 Subject: [PATCH 08/11] Networkx installation error fix https://github.com/leggedrobotics/elevation_mapping_cupy/issues/87 Newest version of Networkx only available for Python >3.9. --- docker/Dockerfile.x64 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile.x64 b/docker/Dockerfile.x64 index 05cad5c7..7864e918 100644 --- a/docker/Dockerfile.x64 +++ b/docker/Dockerfile.x64 @@ -16,7 +16,7 @@ RUN apt-get update && apt-get install -y \ git # Install PyTorch -RUN pip3 install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118 +RUN pip3 install networkx==3.0 torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118 COPY requirements.txt /tmp/requirements.txt RUN pip3 install -r /tmp/requirements.txt From 8de87cebafc10bff7b96fe879c6902b3ccec91a2 Mon Sep 17 00:00:00 2001 From: yyagin Date: Wed, 24 Apr 2024 12:21:32 +0300 Subject: [PATCH 09/11] Add networkx dependency --- requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/requirements.txt b/requirements.txt index 76a0b9d2..fa3f3767 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,6 +7,7 @@ simple-parsing scikit-image==0.19 matplotlib catkin-tools +networkx==3.0 # cupy ###### Requirements with Version Specifiers ######` From dd3d9a2f1086f67d79a6a4fe16829a48049d1a61 Mon Sep 17 00:00:00 2001 From: yyagin Date: Wed, 24 Apr 2024 12:22:05 +0300 Subject: [PATCH 10/11] Install requirements before PyTorch --- docker/Dockerfile.x64 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile.x64 b/docker/Dockerfile.x64 index 7864e918..26bb85f4 100644 --- a/docker/Dockerfile.x64 +++ b/docker/Dockerfile.x64 @@ -16,9 +16,9 @@ RUN apt-get update && apt-get install -y \ git # Install PyTorch -RUN pip3 install networkx==3.0 torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118 COPY requirements.txt /tmp/requirements.txt RUN pip3 install -r /tmp/requirements.txt +RUN pip3 install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118 # Install other Python packages RUN pip3 install cupy-cuda11x scikit-learn From 0126df19f0d3a57f940eb08e4481d9feed52ef51 Mon Sep 17 00:00:00 2001 From: Takahiro Date: Sat, 27 Apr 2024 23:45:13 +0200 Subject: [PATCH 11/11] Added scales param to max_layer_filter. --- .../script/elevation_mapping_cupy/plugins/max_layer_filter.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py index 1c4e7853..34783ebd 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/max_layer_filter.py @@ -29,6 +29,7 @@ def __init__( reverse: list = [True], min_or_max: str = "max", thresholds: list = [False], + scales: list = [1.0], default_value: float = 0.0, **kwargs, ): @@ -37,6 +38,7 @@ def __init__( self.reverse = reverse self.min_or_max = min_or_max self.thresholds = thresholds + self.scales = scales self.default_value = default_value def __call__( @@ -84,6 +86,8 @@ def __call__( layer = cp.where(layer == 0, default_layer, layer) if self.reverse[it]: layer = 1.0 - layer + if len(self.scales) > it and isinstance(self.scales[it], float): + layer = layer * float(self.scales[it]) if isinstance(self.thresholds[it], float): layer = cp.where(layer > float(self.thresholds[it]), 1, 0) layers.append(layer)