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/README.md b/README.md index e7321e72..057424df 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 @@ -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,19 +89,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 +109,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/docker/Dockerfile.x64 b/docker/Dockerfile.x64 index 05cad5c7..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 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 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/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 a5a96577..fff72063 100644 --- a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml +++ b/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 \ No newline at end of file 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/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 @@ - + 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..7b0211df --- /dev/null +++ b/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) 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..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 @@ -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) 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/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 290246cc..072a117c 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; @@ -551,6 +552,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; } 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 ######`