Merge branch 'main' into dev/nature_hiking/feat/distortion

.gitignore

@@ -8,4 +8,5 @@ docs/build
 _build
 .idea*
 .vscode*
-*.egg-info
+*.egg-info
+elevation_mapping_cupy/compile_commands.json

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
 
@@ -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,15 +63,17 @@ 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}
 }
 ```
 
-## Quick instructions to run:
+## Quick instructions to run
 
 ### Installation
 
@@ -87,27 +89,28 @@ 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
 catkin build convex_plane_decomposition_ros  # If you want to use plane segmentation
 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

docker/Dockerfile.x64

@@ -16,9 +16,9 @@ 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
 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

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
+# 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

elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml

@@ -18,12 +18,12 @@ image_channel_fusions:
 
 publishers:
   elevation_map_raw:
-    layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly', 'friction', 'stiffness', 'risk', 'upper_bound']
+    layers: ['elevation', 'traversability', 'variance', 'rgb', 'upper_bound']
     basic_layers: ['elevation', 'traversability']
     fps: 5.0
 
   elevation_map_recordable:
-    layers: ['elevation', 'traversability', 'variance', 'rgb', 'anomaly', 'friction', 'stiffness', 'risk']
+    layers: ['elevation', 'traversability', 'variance', 'rgb']
     basic_layers: ['elevation', 'traversability']
     fps: 2.0
 
@@ -78,16 +78,4 @@ subscribers:
 
   rear_bpearl:
     topic_name: /robot_self_filter/bpearl_rear/point_cloud
-    data_type: pointcloud
-
-  traversability:
-      topic_name: "/hdr_camera/semantic_image"
-      camera_info_topic_name: "/hdr_camera/semantic_camera_info"
-      channel_info_topic_name: "/hdr_camera/image_channel_info"
-      data_type: image
-
-  anomaly:
-    topic_name: "/wild_visual_navigation_node/front/traversability"
-    camera_info_topic_name: "/wild_visual_navigation_node/front/camera_info"
-    channel_info_topic_name: "/wild_visual_navigation_node/front/channel_info"
-    data_type: image
+    data_type: pointcloud

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_;
 };
 

elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch

@@ -3,7 +3,7 @@
 <launch>
 
     <!-- Launch elevation mapping turtle sim. -->
-    <include file="$(find elevation_mapping_cupy)/launch/turtlesim_example.launch">
+    <include file="$(find elevation_mapping_cupy)/launch/turtlesim_simple_example.launch">
         <arg name="rviz_config" value="$(find elevation_mapping_cupy)/rviz/turtle_segmentation_example.rviz"/>
     </include>
 

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/erosion.py

@@ -0,0 +1,113 @@
+#
+# 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,
+        default_layer_name: str = "traversability",
+        **kwargs,
+    ):
+        super().__init__()
+        self.input_layer_name = input_layer_name
+        self.kernel_size = kernel_size
+        self.iterations = iterations
+        self.reverse = reverse
+        self.default_layer_name = default_layer_name
+
+    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,
+        )
+        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
+        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)

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.
     """
 
@@ -28,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,
     ):
@@ -36,32 +38,9 @@ def __init__(
         self.reverse = reverse
         self.min_or_max = min_or_max
         self.thresholds = thresholds
+        self.scales = scales
         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,
@@ -107,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)