diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..cf7fa5b6 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,70 @@ +// See https://aka.ms/vscode-remote/devcontainer.json for format details. +{ + "name": "Demo Elevation Mapping CUPY Workspace", + "dockerFile": "../docker/Dockerfile.x64", + "context": "../", + "remoteUser": "ros", + "runArgs": [ + "--network=host", + "--cap-add=SYS_PTRACE", + "--cap-add=SYS_NICE", + "--security-opt=seccomp:unconfined", + "--security-opt=apparmor:unconfined", + // "--volume=/tmp/.X11-unix:/tmp/.X11-unix", + "--volume=/mnt/wslg:/mnt/wslg", + "--ipc=host", + "--pid=host", + "--runtime=nvidia", + "--gpus=all", + "--privileged", + "--ulimit=rtprio=98", + "--ulimit=rttime=-1", + "--ulimit=memlock=8428281856", + "--name=emcupy-ros2-devcontainer" + ], + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}", // Needed for GUI try ":0" for windows + "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", + "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", + "PULSE_SERVER": "${localEnv:PULSE_SERVER}", + "LIBGL_ALWAYS_SOFTWARE": "1" // Needed for software rendering of opengl + }, + "mounts": [ + "source=/dev,target=/dev,type=bind,consistency=cached" + // "source=~/bag_files,target=/workspaces/vscode-container-workspace/bag_files,type=bind,consistency=cached" + ], + "customizations": { + "vscode": { + "settings": { + "remote.autoForwardPorts": false, + "remote.autoForwardPortsSource": "output", + "otherPortsAttributes": { "onAutoForward" : "ignore" } + }, + "extensions": [ + "althack.ament-task-provider", + "betwo.b2-catkin-tools", + "DotJoshJohnson.xml", + "ms-azuretools.vscode-docker", + "ms-vscode.cmake-tools", + "ms-python.python", + "ms-vscode.cpptools", + "redhat.vscode-yaml", + "smilerobotics.urdf", + "streetsidesoftware.code-spell-checker", + "twxs.cmake", + "yzhang.markdown-all-in-one", + "zachflower.uncrustify", + "mhutchie.git-graph", + "eamodio.gitlens", + "ms-vscode.cpptools-extension-pack", + "usernamehw.errorlens", + "ms-iot.vscode-ros", + "alefragnani.Bookmarks", + "ms-vscode.live-server" + + ] + } + }, + "workspaceMount": "source=${localWorkspaceFolder}/,target=/home/ros/workspace/src/elevation_mapping_cupy/,type=bind", + "workspaceFolder": "/home/ros/workspace/src/elevation_mapping_cupy/" +} \ No newline at end of file diff --git a/README.md b/README.md index 057424df..7365b632 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,140 @@ -# Elevation Mapping cupy +# ROS2 Elevation Mapping Cupy + **Status**: Under Development 🚧 +## Features +- **Point cloud-based map update**: *Functional* +- **Image-based map update**: *Ongoing development* +- **C++ Node**: *Functional* +- **Python Node**: *Functional* +- **Docker & VS Code Devcontainer**: *Provided* + + + +## Installation +A docker file, installation and build scripts, and a VS Code Dev Container have all been provided to ease integration and development. +This has been tested with Ubuntu 22.04, ROS 2 Humble, Zenoh RMW, CUDA 12.1, and PyTorch 2.6.0. +Some dependency issues with numpy, transforms3d, scipy, and ros2_numpy arose during setup so if versions of any of these packages are changed you may run into issues. +The package.xml has been updated to ensure most dependencies are automatically installed via rosdep and some extra rosdep entries are provided to ensure proper versioning. +This is not possible for some packages, e.g. pytorch, due to need for providing an --extra-index-url during installation. +Therefore, this requirement is satsified inside of the docker build. + +To test out this package with the turtlebot3 Gazebo (classic) simulation you will need to install: +- VS Code +- Docker +- NVIDIA Container Toolkit +- NVIDIA CUDA Toolkit + +### Visual Studio Code +```bash +sudo snap install --classic code +``` + +### Docker Installation +```bash +# Add Docker's official GPG key +sudo apt-get update +sudo apt-get install ca-certificates curl gnupg -y +sudo install -m 0755 -d /etc/apt/keyrings +curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg +sudo chmod a+r /etc/apt/keyrings/docker.gpg + +# Add Docker repository +echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu $(. /etc/os-release && echo "$VERSION_CODENAME") stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null +sudo apt-get update + +# Install Docker +sudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin -y + +``` + +### NVIDIA Container Toolkit Installation +```bash +# Configure the repository +curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg +curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list + +# Update and install +sudo apt-get update +sudo apt-get install -y nvidia-container-toolkit + +# Configure Docker for NVIDIA +sudo nvidia-ctk runtime configure --runtime=docker +sudo systemctl restart docker + +# Change Docker root folder +cd +mkdir docker +sudo tee /etc/docker/daemon.json > /dev/null </ +git clone -b ros2_humble https://github.com/jwag/elevation_mapping_cupy.git +``` + +#### Building the Docker Workspace Container +- Open the folder with VS Code +- Select **"Dev Containers: Reopen in container"** in the bottom left from the blue button which will build the docker image. +- Setup the workspace + ```bash + ./docker/setup.sh + ``` +- Build the workspace + ```bash + ./docker/build.sh + +#### Run The Turtlebot3 Demo +The docker should set the environmental variable `TURTLEBOT3_MODEL=waffle_realsense_depth` to select the correct version of the turtlebot to simulate. + +In the first terminal start the zenoh router: +```bash +ros2 run rmw_zenoh_cpp rmw_zenohd +``` + +In a second terminal launch the turtlebot3 in Gazebo with the following command: +```bash +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +``` + +In a third terminal launch the elevation mapping node with the configs for the turtle. Set use_python_node to true to override the default use of the cpp node if you wish: +```bash +ros2 launch elevation_mapping_cupy elevation_mapping_turtle.launch.py use_python_node:=false +``` + +In a fourth terminal run the turtlebot3 teleop node if you want to drive the turtlebot around using the keyboard: +```bash +ros2 run turtlebot3_teleop teleop_keyboard +``` + +--- + +# Elevation Mapping cupy (*Instructions Not Updated for ROS2*) ![python tests](https://github.com/leggedrobotics/elevation_mapping_cupy/actions/workflows/python-tests.yml/badge.svg) diff --git a/docker/Dockerfile.x64 b/docker/Dockerfile.x64 index 26bb85f4..e0fa08c9 100644 --- a/docker/Dockerfile.x64 +++ b/docker/Dockerfile.x64 @@ -1,46 +1,253 @@ -FROM nvidia/cuda:11.6.2-cudnn8-devel-ubuntu20.04 - -# Set noninteractive mode for apt-get -ENV DEBIAN_FRONTEND=noninteractive - -# Preconfigure tzdata -RUN echo "Etc/UTC" > /etc/timezone && \ - ln -fs /usr/share/zoneinfo/Etc/UTC /etc/localtime && \ - apt-get update && apt-get install -y tzdata && \ - dpkg-reconfigure --frontend noninteractive tzdata - -# Install Python -RUN apt-get update && apt-get install -y \ - python3.8 \ - python3-pip \ - git - -# Install PyTorch -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 -RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git' - -# Install ROS -RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 -RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' - -RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-noetic-desktop-full - -# Install ROS packages -RUN apt install -y ros-noetic-pybind11-catkin \ - ros-noetic-grid-map-core ros-noetic-grid-map-msgs ros-noetic-grid-map-ros ros-noetic-grid-map-rviz-plugin \ - libopencv-dev \ - libeigen3-dev \ - libgmp-dev \ - libmpfr-dev \ - libboost-all-dev \ - ros-noetic-turtlebot3-gazebo ros-noetic-turtlebot3-teleop \ - ros-noetic-ros-numpy - -RUN pip3 install catkin-tools -ENV TURTLEBOT3_MODEL=waffle \ No newline at end of file +# syntax=docker/dockerfile:1.4 + +# Base image +FROM nvidia/cuda:12.1.1-cudnn8-devel-ubuntu22.04 AS base + +# Metadata +LABEL description="ROS2 environment with CUDA support" +LABEL version="1.0" + +# Build arguments +ARG ROS_DISTRO=humble +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID +ARG RMW_NAME=zenoh +ARG INSTALL_EMCUPY_ROSDEPS=true + +# Environment variables +ENV DEBIAN_FRONTEND=noninteractive \ + LANG=en_US.UTF-8 \ + LC_ALL=${LANG}\ + TZ=UTC \ + PYTHONUNBUFFERED=1 \ + ROS_DISTRO=${ROS_DISTRO} \ + ROS_ROOT=/opt/ros/${ROS_DISTRO} \ + AMENT_PREFIX_PATH=/opt/ros/${ROS_DISTRO} \ + COLCON_PREFIX_PATH=/opt/ros/${ROS_DISTRO} \ + LD_LIBRARY_PATH=/opt/ros/${ROS_DISTRO}/lib:/usr/local/cuda/lib64 \ + PATH=/opt/ros/${ROS_DISTRO}/bin:/usr/src/tensorrt/bin:/usr/local/cuda/bin:$PATH \ + PYTHONPATH=/opt/ros/${ROS_DISTRO}/lib/python3.10/site-packages \ + # Used by various ROS2 packages + RMW_IMPLEMENTATION=rmw_${RMW_NAME}_cpp \ + # Should be the same as above but with dashes instead of underscores + RMW_IMPLEMENTATION_DASH=rmw-${RMW_NAME}-cpp + +# Install basic utilities and dependencies +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + apt update && apt install -y --no-install-recommends \ + locales \ + tzdata \ + curl \ + gnupg2 \ + lsb-release \ + sudo \ + software-properties-common \ + wget \ + git \ + git-lfs \ + nano \ + && locale-gen ${LANG} \ + && update-locale LC_ALL=${LC_ALL} LANG=${LANG}\ + && ln -fs /usr/share/zoneinfo/${TZ} /etc/localtime \ + && dpkg-reconfigure -f ${DEBIAN_FRONTEND} tzdata \ + && apt clean \ + && rm -rf /var/lib/apt/lists/* + +# Install ROS2 +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null \ + && apt update && apt install -y --no-install-recommends \ + ros-${ROS_DISTRO}-ros-base \ + python3-argcomplete \ + ros-${ROS_DISTRO}-${RMW_IMPLEMENTATION_DASH} \ + && apt clean \ + && rm -rf /var/lib/apt/lists/* + +# Development stage +FROM base AS dev + +# Install development tools and dependencies +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + apt update && apt install -y --no-install-recommends \ + bash-completion \ + build-essential \ + cmake \ + gdb \ + openssh-client \ + python3-pip \ + vim \ + doxygen \ + graphviz \ + python3-sphinx \ + python3-breathe \ + ros-dev-tools \ + ros-${ROS_DISTRO}-ament-* \ + python3-rosdep \ + libxine2-dev \ + libtiff5-dev \ + libpostproc-dev \ + libopencv-dev \ + && apt clean \ + && rm -rf /var/lib/apt/lists/* + +# Pytorch +# Used by elevation_mapping_cupy and not rosdep resolvable for cuda version +RUN python3 -m pip install -U --extra-index-url https://download.pytorch.org/whl/cu121 \ + torch \ + torchvision \ + torchaudio + +# Install Python packages +RUN python3 -m pip install \ + # colcon extension to enable easier workspace cleaning + colcon-clean\ + # Almost all ros2 packages + rosdoc2\ + # sphinx is used across packages for documentation + sphinx_rtd_theme \ + sphinx-multiversion \ + sphinx-copybutton\ + sphinx-tabs\ + # For VS Code python code formatting I think? + autopep8\ + # Used by multiple packages for linting I think? + flake8-builtins\ + flake8-comprehensions\ + flake8-docstrings\ + flake8-import-order\ + flake8-class-newline\ + flake8-blind-except\ + flake8-quotes + # For Python code formatting. Not sure what package uses it + # black==21.12b0\ +#### For elevation_mapping_cupy #### +RUN if [ "$INSTALL_EMCUPY_ROSDEPS" = true ]; then \ + python3 -m pip install \ + # rosdep resolves with apt python-ruamel.yaml + # ruamel.yaml\ + # rosdep resolves with apt python3-sklearn - might not be needed + scikit-learn\ + # rosdep resolves with apt python3-shapely + # shapely\ + # rosdep resolves with apt python3-opencv + opencv-python\ + # rosdep resolves using extra rosdeps + cupy-cuda12x\ + # rosdep resolves using extra rosdeps + simple-parsing\ + # rosdep resolves using extra rosdeps + "numpy<2.0.0"; \ + fi + +# Needed for elevation_mapping_cupy to not have run-time errors +RUN python3 -m pip install transforms3d scipy -U + +# Set up non-root user +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME + +# Set up autocompletion and source ROS environment for user +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /home/$USERNAME/.bashrc \ + && echo "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" >> /home/$USERNAME/.bashrc + +# For building GPU supported containers? +# Install NVIDIA Container Toolkit +RUN distribution=$(. /etc/os-release;echo $ID$VERSION_ID) \ + && curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - \ + && curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list \ + && apt update && apt install -y nvidia-container-toolkit \ + && apt clean \ + && rm -rf /var/lib/apt/lists/* + + +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + if [ "$INSTALL_EMCUPY_ROSDEPS" = true ]; then \ + apt update && DEBIAN_FRONTEND=${DEBIAN_FRONTEND} apt install -y --no-install-recommends \ + # resolved by rosdep as boost + libboost-all-dev\ + ### For elevation_mapping_cupy #### + # rosdep grid_map_msgs + ros-${ROS_DISTRO}-grid-map-msgs\ + # rosdep grid_map_ros + ros-${ROS_DISTRO}-grid-map-ros\ + # rosdep image_transport + ros-${ROS_DISTRO}-image-transport\ + # rosdep pcl_ros + ros-${ROS_DISTRO}-pcl-ros\ + # rosdep cv_bridge + ros-${ROS_DISTRO}-cv-bridge\ + # rosdep tf-transformations + ros-${ROS_DISTRO}-tf-transformations\ + # rosdep rviz2 + ros-${ROS_DISTRO}-rviz2\ + # replacing with rosdep gazebo_ros_pkgs + ros-${ROS_DISTRO}-gazebo-ros\ + # rosdep grid_map_cv + ros-${ROS_DISTRO}-grid-map-cv\ + # rosdep grid_map_core + ros-${ROS_DISTRO}-grid-map-core\ + # rosdep grid_map_demos + ros-${ROS_DISTRO}-grid-map-demos\ + ros-${ROS_DISTRO}-point-cloud-transport\ + python3-shapely\ + python3-scipy\ + python3-ruamel.yaml\ + ################################# + ### For debugging elevation_mapping_cupy by setting up turtlebot3_simulations#### + # ros-${ROS_DISTRO}-turtlebot3*\ + ros-${ROS_DISTRO}-camera-calibration-parsers\ + ros-${ROS_DISTRO}-camera-info-manager\ + ros-${ROS_DISTRO}-gazebo-plugins\ + ros-${ROS_DISTRO}-turtlebot3-msgs\ + ros-${ROS_DISTRO}-turtlebot3-teleop\ + # replacing with rosdep gazebo_ros_pkgs + ros-${ROS_DISTRO}-gazebo-ros-pkgs\ + ############################################# + && apt clean && \ + rm -rf /var/lib/apt/lists/* ;\ + fi +# Development stage +FROM dev as dev2 +ARG WORKSPACE="/home/${USERNAME}/workspace" + +RUN mkdir -p ${WORKSPACE}/src/elevation_mapping_cupy && chown -R ${USER_UID}:${USER_GID} /home/${USERNAME} +RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc +WORKDIR ${WORKSPACE} + + +# Setup rosdep +RUN rosdep init +# COPY docker/extra_rosdeps.yaml /etc/ros/rosdep/sources.list.d/emcupy-rosdeps-fixed.yaml +# Setup rosdep with extra rosdeps and install them for elevation_mapping_cupy +# RUN --mount=type=cache,target=/home/${USERNAME}/.ros/rosdep/sources.cache \ +# RUN if [ "$INSTALL_EMCUPY_ROSDEPS" = true ]; then \ +# echo "yaml file:///etc/ros/rosdep/sources.list.d/emcupy-rosdeps-fixed.yaml" | tee /etc/ros/rosdep/sources.list.d/01-emcupy-rosdeps.list && \ +# rosdep update && \ +# # This install doesn't seem to actually install anything, so removing +# apt update && \ +# rosdep install --from-paths . --ignore-src -y -r; \ +# fi +# Now add the same file again, but this one is symlinked so will change if you change the file in your workspace +RUN ln -s ${WORKSPACE}/src/elevation_mapping_cupy/docker/extra_rosdeps.yaml /etc/ros/rosdep/sources.list.d/emcupy-rosdeps.yaml && \ + echo "yaml file:///etc/ros/rosdep/sources.list.d/emcupy-rosdeps.yaml" | tee /etc/ros/rosdep/sources.list.d/00-emcupy-rosdeps.list; + + +# Switch to non-root user +USER $USERNAME + +# Health check +HEALTHCHECK --interval=30s --timeout=10s --start-period=5s --retries=3 \ + CMD ros2 topic list > /dev/null 2>&1 || exit 1 + +# Set ID to value not used by others on your network +ENV ROS_DOMAIN_ID=12 + +# For elevation_mapping_cupy +ENV TURTLEBOT3_MODEL=waffle_realsense_depth + +# Set the default command to bash +CMD ["bash"] \ No newline at end of file diff --git a/docker/build.sh b/docker/build.sh index 1d92a220..6d67ad53 100755 --- a/docker/build.sh +++ b/docker/build.sh @@ -1,3 +1,22 @@ -#! /bin/bash -home=`realpath "$(dirname "$0")"/../` -cd $home && sudo docker build -t elevation_mapping_cupy -f docker/Dockerfile.x64 --no-cache . \ No newline at end of file +#!/bin/bash +set -e +cd ~/workspace +# Set the default build type +source /opt/ros/$ROS_DISTRO/setup.bash +BUILD_TYPE=RelWithDebInfo #Debug, Release, RelWithDebInfo, MinSizeRel +colcon build \ + --continue-on-error \ + --parallel-workers $(nproc) \ + --merge-install \ + --symlink-install \ + --event-handlers console_cohesion+ \ + --base-paths src \ + --cmake-args \ + "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" \ + "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + "-DBUILD_TESTING=OFF"\ + "-DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined""\ + -Wall -Wextra -Wpedantic -Wshadow \ + --packages-skip \ + convex_plane_decomposition \ + convex_plane_decomposition_ros \ No newline at end of file diff --git a/docker/extra_rosdeps.yaml b/docker/extra_rosdeps.yaml new file mode 100644 index 00000000..5081f24b --- /dev/null +++ b/docker/extra_rosdeps.yaml @@ -0,0 +1,12 @@ +numpy_lessthan_2: + ubuntu: + pip: + packages: ["numpy<2.0.0"] +simple-parsing: + ubuntu: + pip: + packages: [simple-parsing] +cupy-cuda12x: + ubuntu: + pip: + packages: [cupy-cuda12x] \ No newline at end of file diff --git a/docker/setup.sh b/docker/setup.sh new file mode 100755 index 00000000..128e53bc --- /dev/null +++ b/docker/setup.sh @@ -0,0 +1,7 @@ +#!/bin/bash +cd ~/workspace +vcs import < src/elevation_mapping_cupy/docker/src.repos src/ --recursive -w $(($(nproc)/2)) + +sudo apt update +rosdep update +rosdep install --from-paths src --ignore-src -y -r \ No newline at end of file diff --git a/docker/src.repos b/docker/src.repos new file mode 100644 index 00000000..b0d9f6c3 --- /dev/null +++ b/docker/src.repos @@ -0,0 +1,15 @@ +repositories: + # elevation_mapping_cupy: + # type: git + # url: https://github.com/jwag/elevation_mapping_cupy.git + # version: ros2_humble + # ros2_numpy needed by elevation_mapping_cupy + ros2_numpy: + type: git + url: https://github.com/Box-Robotics/ros2_numpy.git + version: humble + # added for testing elevation_mapping_cupy + turtlebot3_simulations: + type: git + url: https://github.com/jwag/turtlebot3_simulations.git + version: humble_realsense_depth diff --git a/elevation_map_msgs/CMakeLists.txt b/elevation_map_msgs/CMakeLists.txt index 2ee13395..08e4467b 100644 --- a/elevation_map_msgs/CMakeLists.txt +++ b/elevation_map_msgs/CMakeLists.txt @@ -4,7 +4,7 @@ project(elevation_map_msgs) find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) # Added dependency +find_package(std_msgs REQUIRED) # Added std_msgs set(msg_files "msg/Statistics.msg" @@ -19,8 +19,37 @@ set(srv_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} - DEPENDENCIES geometry_msgs std_msgs # Combined dependencies + DEPENDENCIES geometry_msgs std_msgs std_msgs # Added std_msgs ) ament_export_dependencies(rosidl_default_runtime) + ament_package() + +# cmake_minimum_required(VERSION 3.8) +# project(elevation_map_msgs) + +# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +# add_compile_options(-Wall -Wextra -Wpedantic) +# endif() + +# # find dependencies +# find_package(ament_cmake REQUIRED) +# find_package(rosidl_default_generators REQUIRED) +# find_package(builtin_interfaces REQUIRED) +# find_package(std_msgs REQUIRED) +# find_package(geometry_msgs REQUIRED) +# find_package(action_msgs REQUIRED) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/Statistics.msg" +# "msg/ChannelInfo.msg" +# "srv/CheckSafety.srv" +# "srv/Initialize.srv" +# DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs +# ) + +# ament_export_dependencies(rosidl_default_runtime) + +# ament_package() + diff --git a/elevation_map_msgs/package.xml b/elevation_map_msgs/package.xml index eec97b14..731eaebb 100644 --- a/elevation_map_msgs/package.xml +++ b/elevation_map_msgs/package.xml @@ -7,19 +7,25 @@ Takahiro Miki MIT + ament_cmake - + geometry_msgs rosidl_default_generators - std_msgs - + std_msgs + geometry_msgs rosidl_default_runtime - std_msgs + std_msgs rosidl_interface_packages + + ament_lint_auto + ament_lint_common + - ament_cmake + ament_cmake + rosidl_interface_packages diff --git a/elevation_mapping_cupy/CMakeLists.txt b/elevation_mapping_cupy/CMakeLists.txt new file mode 100644 index 00000000..e2a66c5c --- /dev/null +++ b/elevation_mapping_cupy/CMakeLists.txt @@ -0,0 +1,132 @@ +cmake_minimum_required(VERSION 3.8) +project(elevation_mapping_cupy) + +# # Enable C++11 (or higher if needed for ROS 2 and pybind11) +# set(CMAKE_CXX_STANDARD 11) +# set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# Compiler options +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Additional dependencies +# find_package(Python COMPONENTS Interpreter Development) +find_package(PythonInterp 3 REQUIRED) +find_package(pybind11 CONFIG REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) + +# Find pybind11 +message([MAIN] "Found pybind11 v${pybind11_VERSION}: ${pybind11_INCLUDE_DIRS}") +message([MAIN] "pybind11_INCLUDE_DIRS = ${pybind11_INCLUDE_DIRS}") +message([MAIN] "pybind11_LIBRARIES = ${pybind11_LIBRARIES}") + +# Find ROS 2 dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(message_filters REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(elevation_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(image_transport REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(python_cmake_module REQUIRED) +find_package(point_cloud_transport REQUIRED) + +# List dependencies for ament_target_dependencies +set(dependencies + rclcpp + rclpy + std_msgs + std_srvs + builtin_interfaces + geometry_msgs + sensor_msgs + elevation_map_msgs + grid_map_msgs + grid_map_ros + image_transport + pcl_ros + message_filters + tf2_eigen + point_cloud_transport +) + +# Include directories +include_directories( + include + ${PYTHON_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${pybind11_INCLUDE_DIRS} + ${elevation_map_msgs_INCLUDE_DIRS} +) + +# Declare C++ library +add_library(elevation_mapping_ros + src/elevation_mapping_wrapper.cpp + src/elevation_mapping_ros.cpp +) + +# Link the library with necessary dependencies +target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES} pybind11::embed) +ament_target_dependencies(elevation_mapping_ros ${dependencies}) + +# Declare C++ executable +add_executable(elevation_mapping_node src/elevation_mapping_node.cpp) + +# Link the executable with the library and dependencies +target_link_libraries(elevation_mapping_node elevation_mapping_ros ${OpenCV_LIBRARIES} pybind11::embed) +ament_target_dependencies(elevation_mapping_node ${dependencies}) + + +# Install targets Not sure if these other argrs are necessary +# install( +# TARGETS elevation_mapping_ros elevation_mapping_node +# DESTINATION lib/${PROJECT_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +# ) + +install(TARGETS elevation_mapping_ros elevation_mapping_node + DESTINATION lib/${PROJECT_NAME} +) + +# install(PROGRAMS +# DESTINATION lib/${PROJECT_NAME} +# ) + +# Install launch, config, and rviz directories +install( + DIRECTORY launch config rviz + DESTINATION share/${PROJECT_NAME} +) + +# TODO: Understand if this line is necessary +# _ament_cmake_python_register_environment_hook() +# For use as a Python module outside of ROS 2 +ament_python_install_package(${PROJECT_NAME}) + +# Install the Python ROS 2 modules +install(PROGRAMS + scripts/elevation_mapping_node.py + DESTINATION lib/${PROJECT_NAME} +) + +# Ament package declaration +ament_package() diff --git a/elevation_mapping_cupy/README.md b/elevation_mapping_cupy/README.md index f7010dff..671de643 100644 --- a/elevation_mapping_cupy/README.md +++ b/elevation_mapping_cupy/README.md @@ -1,12 +1,28 @@ -## Example 1: Turtle Simple Example -Install the realsense wrapper for [TurtleBot3]([ros2_turtlebot3_waffle_intel_realsense](https://github.com/mlherd/ros2_turtlebot3_waffle_intel_realsense)) and place the model in your home .gazebo/models/ folder. -You can launch the turtlebot3 in Gazebo with the following command: +# Example 1: Turtle Simple Example +Set the env in the Dockerfile +```dockerfile +ENV TURTLEBOT3_MODEL=waffle_realsense_depth +``` +or in the terminal ```bash -ros2 launch elevation_mapping_cupy turtle_simple_example.launch.py +export TURTLEBOT3_MODEL=waffle_realsense_depth +``` +If using zenoh as rmw then start one terminal up and run the router +```bash +ros2 run rmw_zenoh_cpp rmw_zenohd +``` + +Now launch the turtlebot3 in Gazebo with the following command: +```bash +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ``` -You can also maunally spawn the robot in Gazebo. +Launch the elevation mapping node with the configs for the turtle. Set use_python_node to true to use it instead of the cpp node: +```bash +ros2 launch elevation_mapping_cupy elevation_mapping_turtle.launch.py use_python_node:=false +``` -Launch the elevation mapping node: +If you want to drive the turtlebot around using the keyboard then run: ```bash -ros2 launch elevation_mapping_cupy turtle_simple_example.launch.py``` \ No newline at end of file +ros2 run turtlebot3_teleop teleop_keyboard +``` \ No newline at end of file diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 3786b25f..5f01ceed 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,11 +1,16 @@ elevation_mapping_node: ros__parameters: #### Basic parameters ######## - resolution: 0.1 # resolution in m. - map_length: 15.0 # map's size in m. + voxel_filter_size: 0.1 + average_weight: 0.5 + drift_compensation_variance_inlier: 0.1 + checker_layer: 'elevation' # layer name for checking the validity of the cell. + initialized_variance: 10.0 # initial variance for each cell. + resolution: 0.1 # resolution in m. + map_length: 20.0 # map's size in m. sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). mahalanobis_thresh: 2.0 # points outside this distance is outlier. - outlier_variance: 0.0`1 # if point is outlier, add this value to the cell. + outlier_variance: 0.01 # if point is outlier, add this value to the cell. drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation. max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety) drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation @@ -20,13 +25,13 @@ elevation_mapping_node: orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens. position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. - min_valid_distance: 4.0 # points with shorter distance will be filtered out. - max_height_range: 10.5 # points higher than this value from sensor will be filtered out + min_valid_distance: 0.1 # points with shorter distance will be filtered out. + max_height_range: 10.5 # points higher than this value from sensor will be filtered out to disable ceiling. ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. update_variance_fps: 5.0 # fps for updating variance. - update_pose_fps: 3.0 # fps for updating pose and shift the center of map. + update_pose_fps: 10.0 # fps for updating pose and shift the center of map. time_interval: 0.1 # Time layer is updated with this interval. map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps. publish_statistics_fps: 1.0 # Publish statistics topic in this fps. @@ -43,21 +48,17 @@ elevation_mapping_node: overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) + # pose_topic: '/dlio/odom_node/odom' + map_frame: 'odom' # The map frame where the odometry source uses. + base_frame: 'base_link' # The robot's base frame. This frame will be a center of the map. + corrected_map_frame: 'odom' - map_frame: 'World' # The map frame where the odometry source uses. - base_frame: 'BASE' # The robot's base frame. This frame will be a center of the map. - corrected_map_frame: 'World' - - - # map_frame: 'World' # The map frame where the odometry source uses. - # base_frame: 'BASE' # The robot's base frame. This frame will be a center of the map. - # corrected_map_frame: 'World' #### Feature toggles ######## enable_edge_sharpen: true enable_visibility_cleanup: true enable_drift_compensation: true - enable_overlap_clearance: true` + enable_overlap_clearance: true enable_pointcloud_publishing: false enable_drift_corrected_TF_publishing: false enable_normal_color: false # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. @@ -71,10 +72,10 @@ elevation_mapping_node: #### Initializer ######## initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic' - initialize_frame_id: 'nearest' # One tf (like ['footprint'] ) initializes a square around it. + initialize_frame_id: ['base_link'] # One tf (like ['footprint'] ) initializes a square around it. initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id. dilation_size_initialize: 2 # dilation size after the init. - initialize_tf_grid_size: 50 # This is not used if number of tf is more than 3. + initialize_tf_grid_size: 1.0 # This is not used if number of tf is more than 3. use_initializer_at_start: true # Use initializer when the node starts. #### Default Plugins ######## diff --git a/elevation_mapping_cupy/config/core/example_setup.yaml b/elevation_mapping_cupy/config/core/example_setup.yaml index c5c4adb5..c67376af 100644 --- a/elevation_mapping_cupy/config/core/example_setup.yaml +++ b/elevation_mapping_cupy/config/core/example_setup.yaml @@ -1,7 +1,7 @@ -/elevation_mapping_node: +elevation_mapping: ros__parameters: #### Plugins ######## - plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/core/plugin_config.yaml' + plugin_config_file: '$(find_package_share elevation_mapping_cupy)/config/core/plugin_config.yaml' #### Channel Fusions ######## pointcloud_channel_fusions: @@ -51,9 +51,9 @@ basic_layers: ['elevation'] fps: 5.0 elevation_map_recordable: - layers: ['elevation', 'traversability'] - basic_layers: ['elevation', 'traversability'] - fps: 2.0 + layers: ['elevation', 'traversability'] + basic_layers: ['elevation', 'traversability'] + fps: 2.0 elevation_map_filter: layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] basic_layers: ['min_filter'] diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_simple.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_simple.yaml index 637a99ae..6d274f25 100644 --- a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_simple.yaml +++ b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_simple.yaml @@ -13,6 +13,7 @@ front_cam: topic_name: '/intel_realsense_r200_depth/points' data_type: pointcloud + channels: ['rgb'] publishers: elevation_map_raw: diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py index 39b2994d..f76756d5 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py @@ -57,8 +57,6 @@ def __init__(self, param: Parameter): Args: param (elevation_mapping_cupy.parameter.Parameter): """ - # Initialize the ROS logger - self.logger = rclpy.logging.get_logger('elevation_map') self.param = param self.data_type = self.param.data_type @@ -243,25 +241,6 @@ def compile_kernels(self): self.min_filtered_mask = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) self.mask = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) - # Log parameter values before kernel initialization - self.logger.info("Initializing add_points_kernel with parameters:") - self.logger.info(f" resolution: {self.resolution}") - self.logger.info(f" cell_n: {self.cell_n}") - self.logger.info(f" sensor_noise_factor: {self.param.sensor_noise_factor}") - self.logger.info(f" mahalanobis_thresh: {self.param.mahalanobis_thresh}") - self.logger.info(f" outlier_variance: {self.param.outlier_variance}") - self.logger.info(f" wall_num_thresh: {self.param.wall_num_thresh}") - self.logger.info(f" max_ray_length: {self.param.max_ray_length}") - self.logger.info(f" cleanup_step: {self.param.cleanup_step}") - self.logger.info(f" min_valid_distance: {self.param.min_valid_distance}") - self.logger.info(f" max_height_range: {self.param.max_height_range}") - self.logger.info(f" cleanup_cos_thresh: {self.param.cleanup_cos_thresh}") - self.logger.info(f" ramped_height_range_a: {self.param.ramped_height_range_a}") - self.logger.info(f" ramped_height_range_b: {self.param.ramped_height_range_b}") - self.logger.info(f" ramped_height_range_c: {self.param.ramped_height_range_c}") - self.logger.info(f" enable_edge_sharpen: {self.param.enable_edge_sharpen}") - self.logger.info(f" enable_visibility_cleanup: {self.param.enable_visibility_cleanup}") - self.add_points_kernel = add_points_kernel( self.resolution, self.cell_n, @@ -339,7 +318,16 @@ def shift_translation_to_map_center(self, t): t -= self.center def update_map_with_kernel(self, points_all, channels, R, t, position_noise, orientation_noise): - """Update map with new measurement.""" + """Update map with new measurement. + + Args: + points_all (cupy._core.core.ndarray): + channels (List[str]): + R (cupy._core.core.ndarray): + t (cupy._core.core.ndarray): + position_noise (float): + orientation_noise (float): + """ self.new_map *= 0.0 error = cp.array([0.0], dtype=cp.float32) error_cnt = cp.array([0], dtype=cp.float32) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_node.py b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_node.py new file mode 120000 index 00000000..4740ecba --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_node.py @@ -0,0 +1 @@ +../scripts/elevation_mapping_node.py \ No newline at end of file diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/fusion/image_latest.py b/elevation_mapping_cupy/elevation_mapping_cupy/fusion/image_latest.py new file mode 100644 index 00000000..e8e7eb14 --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/fusion/image_latest.py @@ -0,0 +1,77 @@ +# +# Copyright (c) 2023, 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 +import string + +from .fusion_manager import FusionBase + + +def latest_correspondences_to_map_kernel(resolution, width, height, alpha): + latest_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(alpha=alpha), + name="latest_correspondences_to_map_kernel", + ) + return latest_correspondences_to_map_kernel + + +class ImageLatest(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "image_latest" + self.cell_n = params.cell_n + self.resolution = params.resolution + + self.latest_correspondences_to_map_kernel = latest_correspondences_to_map_kernel( + resolution=self.resolution, width=self.cell_n, height=self.cell_n, alpha=0.7, + ) + + def __call__( + self, + sem_map_idx, + image, + j, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + semantic_map, + new_map, + ): + self.latest_correspondences_to_map_kernel( + semantic_map, + sem_map_idx, + image[j], + uv_correspondence, + valid_correspondence, + image_height, + image_width, + new_map, + size=int(self.cell_n * self.cell_n), + ) + semantic_map[sem_map_idx] = new_map[sem_map_idx] diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/kernels/kk.py b/elevation_mapping_cupy/elevation_mapping_cupy/kernels/kk.py new file mode 100644 index 00000000..27562dda --- /dev/null +++ b/elevation_mapping_cupy/elevation_mapping_cupy/kernels/kk.py @@ -0,0 +1,1290 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string + + +def sum_kernel( + resolution, width, height, +): + """Sums the semantic values of the classes for the exponentiala verage or for the average. + + Args: + resolution: + width: + height: + + Returns: + + """ + # input the list of layers, amount of channels can slo be input through kernel + sum_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map, raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, map_lay[layer])], feat); + } + } + """ + ).substitute(), + name="sum_kernel", + ) + return sum_kernel + + +def sum_compact_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_compact_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, layer)], feat); + } + } + """ + ).substitute(), + name="sum_compact_kernel", + ) + return sum_compact_kernel + + +def sum_max_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_max_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U max_pt, raw T max_id, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U idx = p[i * pcl_channels[0]]; + U valid = p[i * pcl_channels[0] + 1]; + U inside = p[i * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + // for every max value + for ( W it=0;it=theta_max){ + arg_max = map_lay[layer]; + theta_max = theta; + } + atomicAdd(&newmap[get_map_idx(idx, arg_max)], theta_max); + } + } + """ + ).substitute(), + name="alpha_kernel", + ) + return alpha_kernel + + +def average_kernel( + width, height, +): + average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = feat; + } + """ + ).substitute(), + name="average_map_kernel", + ) + return average_kernel + + +def bayesian_inference_kernel( + width, height, +): + bayesian_inference_kernel = cp.ElementwiseKernel( + in_params=" raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U newmap, raw U sum_mean, raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat_ml = sum_mean[get_map_idx(id, layer)]/cnt; + U feat_old = map[get_map_idx(id, map_lay[layer])]; + U sigma_old = newmap[get_map_idx(id, map_lay[layer])]; + U sigma = 1.0; + U feat_new = sigma*feat_old /(cnt*sigma_old + sigma) +cnt*sigma_old *feat_ml /(cnt*sigma_old+sigma); + U sigma_new = sigma*sigma_old /(cnt*sigma_old +sigma); + map[get_map_idx(id, map_lay[layer])] = feat_new; + newmap[get_map_idx(id, map_lay[layer])] = sigma_new; + } + """ + ).substitute(), + name="bayesian_inference_kernel", + ) + return bayesian_inference_kernel + + +def class_average_kernel( + width, height, alpha, +): + class_average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U prev_val = map[get_map_idx(id, map_lay[layer])]; + if (prev_val==0){ + U val = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + else{ + U val = ${alpha} *prev_val + (1-${alpha}) * newmap[get_map_idx(id, map_lay[layer])]/(cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + } + """ + ).substitute(alpha=alpha,), + name="class_average_kernel", + ) + return class_average_kernel + + +def add_color_kernel( + width, height, +): + add_color_kernel = cp.ElementwiseKernel( + in_params="raw T p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw V color_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = ( color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid && inside){ + unsigned int color = __float_as_uint(p[id * pcl_channels[0] + pcl_chan[layer]]); + atomicAdd(&color_map[get_map_idx(idx, layer*3)], get_r(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3+1)], get_g(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3 + 2)], get_b(color)); + atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1); + } + """ + ).substitute(width=width), + name="add_color_kernel", + ) + return add_color_kernel + + +def color_average_kernel( + width, height, +): + color_average_kernel = cp.ElementwiseKernel( + in_params="raw V color_map, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = (color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + unsigned int cnt = color_map[get_map_idx(id, pcl_channels[1]*3)]; + if (cnt>0){ + // U prev_color = map[get_map_idx(id, map_lay[layer])]; + unsigned int r = color_map[get_map_idx(id, layer*3)]/(1*cnt); + unsigned int g = color_map[get_map_idx(id, layer*3+1)]/(1*cnt); + unsigned int b = color_map[get_map_idx(id, layer*3+2)]/(1*cnt); + //if (prev_color>=0){ + // unsigned int prev_r = get_r(prev_color); + // unsigned int prev_g = get_g(prev_color); + // unsigned int prev_b = get_b(prev_color); + // unsigned int r = prev_r/2 + color_map[get_map_idx(i, layer*3)]/(2*cnt); + // unsigned int g = prev_g/2 + color_map[get_map_idx(i, layer*3+1)]/(2*cnt); + // unsigned int b = prev_b/2 + color_map[get_map_idx(i, layer*3+2)]/(2*cnt); + //} + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + map[get_map_idx(id, map_lay[layer])] = rgb_; + } + """ + ).substitute(), + name="color_average_kernel", + ) + return color_average_kernel + + + +def map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + util_preamble = string.Template( + """ + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ int get_x_idx(float16 x, float16 center) { + int i = (x - center) / ${resolution} + 0.5 * ${width}; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + int i = (y - center) / ${resolution} + 0.5 * ${height}; + return i; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x == 0 || idx_x == ${width} - 1) { + return false; + } + if (idx_y == 0 || idx_y == ${height} - 1) { + return false; + } + return true; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ float transform_p(float16 x, float16 y, float16 z, + float16 r0, float16 r1, float16 r2, float16 t) { + return r0 * x + r1 * y + r2 * z + t; + } + __device__ float z_noise(float16 z){ + return ${sensor_noise_factor} * z * z; + } + + __device__ float point_sensor_distance(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = (x - sx) * (x - sx) + (y - sy) * (y - sy) + (z - sz) * (z - sz); + return d; + } + + __device__ bool is_valid(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = point_sensor_distance(x, y, z, sx, sy, sz); + float dxy = max(sqrt(x * x + y * y) - ${ramped_height_range_b}, 0.0); + if (d < ${min_valid_distance} * ${min_valid_distance}) { + return false; + } + else if (z - sz > dxy * ${ramped_height_range_a} + ${ramped_height_range_c} || z - sz > ${max_height_range}) { + return false; + } + else { + return true; + } + } + + __device__ float ray_vector(float16 tx, float16 ty, float16 tz, + float16 px, float16 py, float16 pz, + float16& rx, float16& ry, float16& rz){ + float16 vx = px - tx; + float16 vy = py - ty; + float16 vz = pz - tz; + float16 norm = sqrt(vx * vx + vy * vy + vz * vz); + if (norm > 0) { + rx = vx / norm; + ry = vy / norm; + rz = vz / norm; + } + else { + rx = 0; + ry = 0; + rz = 0; + } + return norm; + } + + __device__ float inner_product(float16 x1, float16 y1, float16 z1, + float16 x2, float16 y2, float16 z2) { + + float product = (x1 * x2 + y1 * y2 + z1 * z2); + return product; + } + + """ + ).substitute( + resolution=resolution, + width=width, + height=height, + sensor_noise_factor=sensor_noise_factor, + min_valid_distance=min_valid_distance, + max_height_range=max_height_range, + ramped_height_range_a=ramped_height_range_a, + ramped_height_range_b=ramped_height_range_b, + ramped_height_range_c=ramped_height_range_c, + ) + return util_preamble + + +def add_points_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + wall_num_thresh, + max_ray_length, + cleanup_step, + min_valid_distance, + max_height_range, + cleanup_cos_thresh, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + enable_edge_shaped=True, + enable_visibility_cleanup=True, +): + add_points_kernel = cp.ElementwiseKernel( + in_params="raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map", + out_params="raw U p, raw U map, raw T newmap", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (is_valid(x, y, z, t[0], t[1], t[2])) { + if (is_inside(idx)) { + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U num_points = newmap[get_map_idx(idx, 4)]; + if (abs(map_h - z) > (map_v * ${mahalanobis_thresh})) { + atomicAdd(&map[get_map_idx(idx, 1)], ${outlier_variance}); + } + else { + if (${enable_edge_shaped} && (num_points > ${wall_num_thresh}) && (z < map_h - map_v * ${mahalanobis_thresh} / num_points)) { + // continue; + } + else { + T new_h = (map_h * v + z * map_v) / (map_v + v); + T new_v = (map_v * v) / (map_v + v); + atomicAdd(&newmap[get_map_idx(idx, 0)], new_h); + atomicAdd(&newmap[get_map_idx(idx, 1)], new_v); + atomicAdd(&newmap[get_map_idx(idx, 2)], 1.0); + // is Valid + map[get_map_idx(idx, 2)] = 1; + // Time layer + map[get_map_idx(idx, 4)] = 0.0; + // Upper bound + map[get_map_idx(idx, 5)] = new_h; + map[get_map_idx(idx, 6)] = 0.0; + } + // visibility cleanup + } + } + } + if (${enable_visibility_cleanup}) { + float16 ray_x, ray_y, ray_z; + float16 ray_length = ray_vector(t[0], t[1], t[2], x, y, z, ray_x, ray_y, ray_z); + ray_length = min(ray_length, (float16)${max_ray_length}); + int last_nidx = -1; + for (float16 s=${ray_step}; s < ray_length; s+=${ray_step}) { + // iterate through ray + U nx = t[0] + ray_x * s; + U ny = t[1] + ray_y * s; + U nz = t[2] + ray_z * s; + int nidx = get_idx(nx, ny, center_x[0], center_y[0]); + if (last_nidx == nidx) {continue;} // Skip if we're still in the same cell + else {last_nidx = nidx;} + if (!is_inside(nidx)) {continue;} + + U nmap_h = map[get_map_idx(nidx, 0)]; + U nmap_v = map[get_map_idx(nidx, 1)]; + U nmap_valid = map[get_map_idx(nidx, 2)]; + // traversability + U nmap_trav = map[get_map_idx(nidx, 3)]; + // Time layer + U non_updated_t = map[get_map_idx(nidx, 4)]; + // upper bound + U nmap_upper = map[get_map_idx(nidx, 5)]; + U nmap_is_upper = map[get_map_idx(nidx, 6)]; + + // If point is close or is farther away than ray length, skip. + float16 d = (x - nx) * (x - nx) + (y - ny) * (y - ny) + (z - nz) * (z - nz); + if (d < 0.1 || !is_valid(x, y, z, t[0], t[1], t[2])) {continue;} + + // If invalid, do upper bound check, then skip + if (nmap_valid < 0.5) { + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + continue; + } + // If updated recently, skip + if (non_updated_t < 0.5) {continue;} + + if (nmap_h > nz + 0.01 - min(nmap_v, 1.0) * 0.05) { + // If ray and norm is vertical, skip + U norm_x = norm_map[get_map_idx(nidx, 0)]; + U norm_y = norm_map[get_map_idx(nidx, 1)]; + U norm_z = norm_map[get_map_idx(nidx, 2)]; + float product = inner_product(ray_x, ray_y, ray_z, norm_x, norm_y, norm_z); + if (fabs(product) < ${cleanup_cos_thresh}) {continue;} + U num_points = newmap[get_map_idx(nidx, 3)]; + if (num_points > ${wall_num_thresh} && non_updated_t < 1.0) {continue;} + + // Finally, this cell is penetrated by the ray. + atomicAdd(&map[get_map_idx(nidx, 2)], -${cleanup_step}/(ray_length / ${max_ray_length})); + atomicAdd(&map[get_map_idx(nidx, 1)], ${outlier_variance}); + // Do upper bound check. + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + } + } + } + p[i * 3]= idx; + p[i * 3 + 1] = is_valid(x, y, z, t[0], t[1], t[2]); + p[i * 3 + 2] = is_inside(idx); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + wall_num_thresh=wall_num_thresh, + ray_step=resolution / 2 ** 0.5, + max_ray_length=max_ray_length, + cleanup_step=cleanup_step, + cleanup_cos_thresh=cleanup_cos_thresh, + enable_edge_shaped=int(enable_edge_shaped), + enable_visibility_cleanup=int(enable_visibility_cleanup), + ), + name="add_points_kernel", + ) + return add_points_kernel + + +def error_counting_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + traversability_inlier, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + error_counting_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U p, raw U center_x, raw U center_y, raw U R, raw U t", + out_params="raw U newmap, raw T error, raw T error_cnt", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + // if (!is_valid(z, t[2])) {return;} + if (!is_valid(x, y, z, t[0], t[1], t[2])) {return;} + // if ((x - t[0]) * (x - t[0]) + (y - t[1]) * (y - t[1]) + (z - t[2]) * (z - t[2]) < 0.5) {return;} + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (!is_inside(idx)) { + return; + } + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U map_valid = map[get_map_idx(idx, 2)]; + U map_t = map[get_map_idx(idx, 3)]; + if (map_valid > 0.5 && (abs(map_h - z) < (map_v * ${mahalanobis_thresh})) + && map_v < ${outlier_variance} / 2.0 + && map_t > ${traversability_inlier}) { + T e = z - map_h; + atomicAdd(&error[0], e); + atomicAdd(&error_cnt[0], 1); + atomicAdd(&newmap[get_map_idx(idx, 3)], 1.0); + } + atomicAdd(&newmap[get_map_idx(idx, 4)], 1.0); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + traversability_inlier=traversability_inlier, + ), + name="error_counting_kernel", + ) + return error_counting_kernel + + +def average_map_kernel(width, height, max_variance, initial_variance): + average_map_kernel = cp.ElementwiseKernel( + in_params="raw U newmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U v = map[get_map_idx(i, 1)]; + U valid = map[get_map_idx(i, 2)]; + U new_h = newmap[get_map_idx(i, 0)]; + U new_v = newmap[get_map_idx(i, 1)]; + U new_cnt = newmap[get_map_idx(i, 2)]; + if (new_cnt > 0) { + if (new_v / new_cnt > ${max_variance}) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + else { + map[get_map_idx(i, 0)] = new_h / new_cnt; + map[get_map_idx(i, 1)] = new_v / new_cnt; + map[get_map_idx(i, 2)] = 1; + } + } + if (valid < 0.5) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + """ + ).substitute(max_variance=max_variance, initial_variance=initial_variance), + name="average_map_kernel", + ) + return average_map_kernel + + +def dilation_filter_kernel(width, height, dilation_size): + dilation_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap, raw U newmask", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + newmap[get_map_idx(i, 0)] = h; + if (valid < 0.5) { + U distance = 100; + U near_value = 0; + for (int dy = -${dilation_size}; dy <= ${dilation_size}; dy++) { + for (int dx = -${dilation_size}; dx <= ${dilation_size}; dx++) { + int idx = get_relative_map_idx(i, dx, dy, 0); + if (!is_inside(idx)) {continue;} + U valid = mask[idx]; + if(valid > 0.5 && dx + dy < distance) { + distance = dx + dy; + near_value = map[idx]; + } + } + } + if(distance < 100) { + newmap[get_map_idx(i, 0)] = near_value; + newmask[get_map_idx(i, 0)] = 1.0; + } + } + """ + ).substitute(dilation_size=dilation_size), + name="dilation_filter_kernel", + ) + return dilation_filter_kernel + + +def normal_filter_kernel(width, height, resolution): + normal_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + __device__ float resolution() { + return ${resolution}; + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + if (valid > 0.5) { + int idx_x = get_relative_map_idx(i, 1, 0, 0); + int idx_y = get_relative_map_idx(i, 0, 1, 0); + if (!is_inside(idx_x) || !is_inside(idx_y)) { return; } + float dzdx = (map[idx_x] - h); + float dzdy = (map[idx_y] - h); + float nx = -dzdy / resolution(); + float ny = -dzdx / resolution(); + float nz = 1; + float norm = sqrt((nx * nx) + (ny * ny) + 1); + newmap[get_map_idx(i, 0)] = nx / norm; + newmap[get_map_idx(i, 1)] = ny / norm; + newmap[get_map_idx(i, 2)] = nz / norm; + } + """ + ).substitute(), + name="normal_filter_kernel", + ) + return normal_filter_kernel + + +def polygon_mask_kernel(width, height, resolution): + polygon_mask_kernel = cp.ElementwiseKernel( + in_params="raw U polygon, raw U center_x, raw U center_y, raw int16 polygon_n, raw U polygon_bbox", + out_params="raw U mask", + preamble=string.Template( + """ + __device__ struct Point + { + int x; + int y; + }; + // Given three colinear points p, q, r, the function checks if + // point q lies on line segment 'pr' + __device__ bool onSegment(Point p, Point q, Point r) + { + if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && + q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) + return true; + return false; + } + // To find orientation of ordered triplet (p, q, r). + // The function returns following values + // 0 --> p, q and r are colinear + // 1 --> Clockwise + // 2 --> Counterclockwise + __device__ int orientation(Point p, Point q, Point r) + { + int val = (q.y - p.y) * (r.x - q.x) - + (q.x - p.x) * (r.y - q.y); + if (val == 0) return 0; // colinear + return (val > 0)? 1: 2; // clock or counterclock wise + } + // The function that returns true if line segment 'p1q1' + // and 'p2q2' intersect. + __device__ bool doIntersect(Point p1, Point q1, Point p2, Point q2) + { + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + // General case + if (o1 != o2 && o3 != o4) + return true; + // Special Cases + // p1, q1 and p2 are colinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + // p1, q1 and p2 are colinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + // p2, q2 and p1 are colinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + // p2, q2 and q1 are colinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + return false; // Doesn't fall in any of the above cases + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_idx_x(int idx){ + int idx_x = idx / ${width}; + return idx_x; + } + + __device__ int get_idx_y(int idx){ + int idx_y = idx % ${width}; + return idx_y; + } + + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ float16 round(float16 x) { + return (int)x + (int)(2 * (x - (int)x)); + } + __device__ int get_x_idx(float16 x, float16 center) { + const float resolution = ${resolution}; + const float width = ${width}; + int i = (x - center) / resolution + 0.5 * width; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + const float resolution = ${resolution}; + const float height = ${height}; + int i = (y - center) / resolution + 0.5 * height; + return i; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + // Point p = {get_idx_x(i, center_x[0]), get_idx_y(i, center_y[0])}; + Point p = {get_idx_x(i), get_idx_y(i)}; + Point extreme = {100000, p.y}; + int bbox_min_idx = get_idx(polygon_bbox[0], polygon_bbox[1], center_x[0], center_y[0]); + int bbox_max_idx = get_idx(polygon_bbox[2], polygon_bbox[3], center_x[0], center_y[0]); + Point bmin = {get_idx_x(bbox_min_idx), get_idx_y(bbox_min_idx)}; + Point bmax = {get_idx_x(bbox_max_idx), get_idx_y(bbox_max_idx)}; + if (p.x < bmin.x || p.x > bmax.x || p.y < bmin.y || p.y > bmax.y){ + mask[i] = 0; + return; + } + else { + int intersect_cnt = 0; + for (int j = 0; j < polygon_n[0]; j++) { + Point p1, p2; + int i1 = get_idx(polygon[j * 2 + 0], polygon[j * 2 + 1], center_x[0], center_y[0]); + p1.x = get_idx_x(i1); + p1.y = get_idx_y(i1); + int j2 = (j + 1) % polygon_n[0]; + int i2 = get_idx(polygon[j2 * 2 + 0], polygon[j2 * 2 + 1], center_x[0], center_y[0]); + p2.x = get_idx_x(i2); + p2.y = get_idx_y(i2); + if (doIntersect(p1, p2, p, extreme)) + { + // If the point 'p' is colinear with line segment 'i-next', + // then check if it lies on segment. If it lies, return true, + // otherwise false + if (orientation(p1, p, p2) == 0) { + if (onSegment(p1, p, p2)){ + mask[i] = 1; + return; + } + } + else if(((p1.y <= p.y) && (p2.y > p.y)) || ((p1.y > p.y) && (p2.y <= p.y))){ + intersect_cnt++; + } + } + } + if (intersect_cnt % 2 == 0) { mask[i] = 0; } + else { mask[i] = 1; } + } + """ + ).substitute(a=1), + name="polygon_mask_kernel", + ) + return polygon_mask_kernel + + + +def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_collision): + """ + This function calculates the correspondence between the image and the map. + It takes in the resolution, width, height, and tolerance_z_collision as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _image_to_map_correspondence_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U K, raw U D, raw U image_height, raw U image_width, raw U center", + out_params="raw U uv_correspondence, raw B valid_correspondence", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ bool is_inside_map(int x, int y) { + return (x >= 0 && y >= 0 && x<${width} && y<${height}); + } + __device__ float get_l2_distance(int x0, int y0, int x1, int y1) { + float dx = x0-x1; + float dy = y0-y1; + return sqrt( dx*dx + dy*dy); + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + + // return if gridcell has no valid height + if (map[get_map_idx(i, 2)] != 1){ + return; + } + + // get current cell position + int y0 = i % ${width}; + int x0 = i / ${width}; + + // gridcell 3D point in worldframe TODO reverse x and y + float p1 = (x0-(${width}/2)) * ${resolution} + center[0]; + float p2 = (y0-(${height}/2)) * ${resolution} + center[1]; + float p3 = map[cell_idx] + center[2]; + + // reproject 3D point into image plane + float u = p1 * P[0] + p2 * P[1] + p3 * P[2] + P[3]; + float v = p1 * P[4] + p2 * P[5] + p3 * P[6] + P[7]; + float d = p1 * P[8] + p2 * P[9] + p3 * P[10] + P[11]; + + // filter point behind image plane + if (d <= 0) { + return; + } + u = u/d; + v = v/d; + + // Check if D is all zeros + bool is_D_zero = (D[0] == 0 && D[1] == 0 && D[2] == 0 && D[3] == 0 && D[4] == 0); + + // Apply undistortion using distortion matrix D if not all zeros + if (!is_D_zero) { + float k1 = D[0]; + float k2 = D[1]; + float p1 = D[2]; + float p2 = D[3]; + float k3 = D[4]; + float fx = K[0]; + float fy = K[4]; + float cx = K[2]; + float cy = K[5]; + float x = (u - cx) / fx; + float y = (v - cy) / fy; + float r2 = x * x + y * y; + float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + float u_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float v_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = fx * u_corrected + cx; + v = fy * v_corrected + cy; + } + + // filter point next to image plane + if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ + return; + } + + int y0_c = y0; + int x0_c = x0; + float total_dis = get_l2_distance(x0_c, y0_c, x1,y1); + float z0 = map[cell_idx]; + float delta_z = z1-z0; + + + // bresenham algorithm to iterate over cells in line between camera center and current gridmap cell + // https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + int dx = abs(x1-x0); + int sx = x0 < x1 ? 1 : -1; + int dy = -abs(y1 - y0); + int sy = y0 < y1 ? 1 : -1; + int error = dx + dy; + + bool is_valid = true; + + // iterate over all cells along line + while (1){ + // assumption we do not need to check the height for camera center cell + if (x0 == x1 && y0 == y1){ + break; + } + + // check if height is invalid + if (is_inside_map(x0,y0)){ + int idx = y0 + (x0 * ${width}); + if (map[get_map_idx(idx, 2)]){ + float dis = get_l2_distance(x0_c, y0_c, x0, y0); + float rayheight = z0 + ( dis / total_dis * delta_z); + if ( map[idx] - ${tolerance_z_collision} > rayheight){ + is_valid = false; + break; + } + } + } + + + // computation of next gridcell index in line + int e2 = 2 * error; + if (e2 >= dy){ + if(x0 == x1){ + break; + } + error = error + dy; + x0 = x0 + sx; + } + if (e2 <= dx){ + if (y0 == y1){ + break; + } + error = error + dx; + y0 = y0 + sy; + } + } + + // mark the correspondence + uv_correspondence[get_map_idx(i, 0)] = u; + uv_correspondence[get_map_idx(i, 1)] = v; + valid_correspondence[get_map_idx(i, 0)] = is_valid; + """ + ).substitute(height=height, width=width, resolution=resolution, tolerance_z_collision=tolerance_z_collision), + name="image_to_map_correspondence_kernel", + ) + return _image_to_map_correspondence_kernel + + +def average_correspondences_to_map_kernel(width, height): + """ + This function calculates the average correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _average_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(), + name="average_correspondences_to_map_kernel", + ) + return _average_correspondences_to_map_kernel + + +def exponential_correspondences_to_map_kernel(width, height, alpha): + """ + This function calculates the exponential correspondences to the map. + It takes in the width, height, and alpha as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _exponential_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)] * (1-${alpha}) + ${alpha} * image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(alpha=alpha), + name="exponential_correspondences_to_map_kernel", + ) + return _exponential_correspondences_to_map_kernel + + +def color_correspondences_to_map_kernel(width, height): + """ + This function calculates the color correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _color_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_rgb, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + + int idx_red = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + int idx_green = image_width * image_height + idx_red; + int idx_blue = image_width * image_height * 2 + idx_red; + + unsigned int r = image_rgb[idx_red]; + unsigned int g = image_rgb[idx_green]; + unsigned int b = image_rgb[idx_blue]; + + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + new_sem_map[get_map_idx(i, map_idx)] = rgb_; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + """ + ).substitute(), + name="color_correspondences_to_map_kernel", + ) + return _color_correspondences_to_map_kernel diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/elevation_mapping_cupy/parameter.py index 5fc3c110..059fbb4c 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/parameter.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/parameter.py @@ -84,10 +84,6 @@ class Parameter(Serializable): (Default: ``20``) checker_layer: Layer used for checking safety. (Default: ``"traversability"``) - min_filter_size: The minimum size for the filter. - (Default: ``5``) - min_filter_iteration: The minimum number of iterations for the filter. - (Default: ``3``) max_drift: The maximum drift for the compensation. (Default: ``0.10``) overlap_clear_range_xy: XY range [m] for clearing overlapped area. This defines the valid area for overlap clearance. (used for multi floor setting) @@ -171,13 +167,13 @@ class Parameter(Serializable): time_interval: float = 0.1 # Time layer is updated with this interval. max_variance: float = 1.0 # maximum variance for each cell. - dilation_size: float = 2 # dilation filter size before traversability filter. - dilation_size_initialize: float = 10 # dilation size after the init. + dilation_size: int = 2 # dilation filter size before traversability filter. + dilation_size_initialize: int = 10 # dilation size after the init. drift_compensation_alpha: float = 1.0 # drift compensation alpha for smoother update of drift compensation. traversability_inlier: float = 0.1 # cells with higher traversability are used for drift compensation. - wall_num_thresh: float = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. - min_height_drift_cnt: float = 100 # drift compensation only happens if the valid cells are more than this number. + wall_num_thresh: int = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. + min_height_drift_cnt: int = 100 # drift compensation only happens if the valid cells are more than this number. max_ray_length: float = 2.0 # maximum length for ray tracing. cleanup_step: float = 0.01 # substitute this value from validity layer at visibility cleanup. @@ -193,9 +189,6 @@ class Parameter(Serializable): max_unsafe_n: int = 20 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe. checker_layer: str = "traversability" # layer used for checking safety - min_filter_size: int = 5 # minimum size for the filter - min_filter_iteration: int = 3 # minimum number of iterations for the filter - max_drift: float = 0.10 # maximum drift for the compensation overlap_clear_range_xy: float = 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) @@ -225,7 +218,7 @@ class Parameter(Serializable): cell_n: int = None # number of cells in the map true_cell_n: int = None # true number of cells in the map - def load_weights(self, filename): + def load_weights(self, filename: str): """ Load weights from a file into the model's parameters. diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/semantic_map.py b/elevation_mapping_cupy/elevation_mapping_cupy/semantic_map.py index a06b7361..390597b6 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/semantic_map.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/semantic_map.py @@ -41,7 +41,7 @@ def __init__(self, param: Parameter): self.new_map = xp.zeros((self.amount_layer_names, self.param.cell_n, self.param.cell_n), param.data_type,) # which layers should be reset to zero at each update, per default everyone, # if a layer should not be reset, it is defined in compile_kernels function - self.delete_new_layers = cp.ones(self.new_map.shape[0], cp.bool8) + self.delete_new_layers = cp.ones(self.new_map.shape[0], cp.bool_) self.fusion_manager = FusionManager(self.param) def clear(self): @@ -94,7 +94,7 @@ def add_layer(self, name): self.new_map = cp.append( self.new_map, cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type), axis=0, ) - self.delete_new_layers = cp.append(self.delete_new_layers, cp.array([1], dtype=cp.bool8)) + self.delete_new_layers = cp.append(self.delete_new_layers, cp.array([1], dtype=cp.bool_)) def pad_value(self, x, shift_value, idx=None, value=0.0): """Create a padding of the map along x,y-axis according to amount that has shifted. diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/traversability_polygon.py b/elevation_mapping_cupy/elevation_mapping_cupy/traversability_polygon.py index f39c99cc..be874f1b 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/traversability_polygon.py +++ b/elevation_mapping_cupy/elevation_mapping_cupy/traversability_polygon.py @@ -46,7 +46,7 @@ def calculate_area(polygon): def calculate_untraversable_polygon(over_thresh): x, y = cp.where(over_thresh > 0.5) points = cp.stack([x, y]).T - convex_hull = MultiPoint(points).convex_hull + convex_hull = MultiPoint(points.get()).convex_hull if convex_hull.is_empty or convex_hull.geom_type == "Point" or convex_hull.geom_type == "LineString": return None else: 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 0fb75b6a..ebdd016e 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 @@ -14,42 +14,51 @@ // Pybind #include // everything needed for embedding - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +// ROS2 +#include +#include +#include +#include +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include // Grid Map -#include -#include +#include +#include #include // PCL #include #include #include +#include // OpenCV #include #include -#include -#include -#include +#include +#include +#include +#include #include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp" @@ -57,135 +66,178 @@ namespace py = pybind11; namespace elevation_mapping_cupy { -class ElevationMappingNode { +class ElevationMappingNode : public rclcpp::Node { public: - ElevationMappingNode(ros::NodeHandle& nh); + ElevationMappingNode(const rclcpp::NodeOptions& options); using RowMatrixXd = Eigen::Matrix; using ColMatrixXf = Eigen::Matrix; - using ImageSubscriber = image_transport::SubscriberFilter; + // using ImageSubscriber = image_transport::SubscriberFilter; + using ImageSubscriber = message_filters::Subscriber; using ImageSubscriberPtr = std::shared_ptr; // Subscriber and Synchronizer for CameraInfo messages - using CameraInfoSubscriber = message_filters::Subscriber; + using CameraInfoSubscriber = message_filters::Subscriber; using CameraInfoSubscriberPtr = std::shared_ptr; - using CameraPolicy = message_filters::sync_policies::ApproximateTime; + using CameraPolicy = message_filters::sync_policies::ApproximateTime; using CameraSync = message_filters::Synchronizer; using CameraSyncPtr = std::shared_ptr; // Subscriber and Synchronizer for ChannelInfo messages - using ChannelInfoSubscriber = message_filters::Subscriber; + using ChannelInfoSubscriber = message_filters::Subscriber; using ChannelInfoSubscriberPtr = std::shared_ptr; - using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime; + using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime; using CameraChannelSync = message_filters::Synchronizer; using CameraChannelSyncPtr = std::shared_ptr; // Subscriber and Synchronizer for Pointcloud messages - using PointCloudSubscriber = message_filters::Subscriber; - using PointCloudSubscriberPtr = std::shared_ptr; - using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; - using PointCloudSync = message_filters::Synchronizer; - using PointCloudSyncPtr = std::shared_ptr; + // using PointCloudSubscriber = message_filters::Subscriber; + // using PointCloudSubscriberPtr = std::shared_ptr; + // using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; + // using PointCloudSync = message_filters::Synchronizer; + // using PointCloudSyncPtr = std::shared_ptr; private: - void readParameters(); - void setupMapPublishers(); - void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key); - void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector& channels); - void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector& channels); - void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key); - void imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); - void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); - // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); - void publishAsPointCloud(const grid_map::GridMap& map) const; - bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); - bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); - bool initializeMap(elevation_map_msgs::Initialize::Request& request, elevation_map_msgs::Initialize::Response& response); - bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); - bool clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); - bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); - void updatePose(const ros::TimerEvent&); - void updateVariance(const ros::TimerEvent&); - void updateTime(const ros::TimerEvent&); - void updateGridMap(const ros::TimerEvent&); - void publishNormalAsArrow(const grid_map::GridMap& map) const; - void initializeWithTF(); - void publishMapToOdom(double error); - void publishStatistics(const ros::TimerEvent&); - void publishMapOfIndex(int index); - - visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; - - ros::NodeHandle nh_; - image_transport::ImageTransport it_; - std::vector pointcloudSubs_; - std::vector imageSubs_; - std::vector cameraInfoSubs_; - std::vector channelInfoSubs_; - std::vector cameraSyncs_; - std::vector cameraChannelSyncs_; - std::vector pointCloudSyncs_; - std::vector mapPubs_; - tf::TransformBroadcaster tfBroadcaster_; - ros::Publisher alivePub_; - ros::Publisher pointPub_; - ros::Publisher normalPub_; - ros::Publisher statisticsPub_; - ros::ServiceServer rawSubmapService_; - ros::ServiceServer clearMapService_; - ros::ServiceServer clearMapWithInitializerService_; - ros::ServiceServer initializeMapService_; - ros::ServiceServer setPublishPointService_; - ros::ServiceServer checkSafetyService_; - ros::Timer updateVarianceTimer_; - ros::Timer updateTimeTimer_; - ros::Timer updatePoseTimer_; - ros::Timer updateGridMapTimer_; - ros::Timer publishStatisticsTimer_; - ros::Time lastStatisticsPublishedTime_; - tf::TransformListener transformListener_; - ElevationMappingWrapper map_; +void readParameters(); +void setupMapPublishers(); +void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key); +void pointcloudtransportCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const std::string& key); + +void inputPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const std::vector& channels); + +void inputImage(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg, + const std::vector& channels); + + +// void imageCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const std::string& key); +// void imageChannelCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const elevation_map_msgs::msg::ChannelInfo::SharedPtr channel_info_msg); +void imageCallback(const sensor_msgs::msg::Image::SharedPtr cloud, const std::string& key); +void imageChannelCallback(const elevation_map_msgs::msg::ChannelInfo::SharedPtr chennel_info, const std::string& key); +void imageInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr image_info, const std::string& key); +// void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); +// // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); +void publishAsPointCloud(const grid_map::GridMap& map) const; +bool getSubmap( const std::shared_ptr request, std::shared_ptr response); + + +// bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); + +void initializeMap(const std::shared_ptr request, std::shared_ptr response); +void clearMap(const std::shared_ptr request, std::shared_ptr response); +void clearMapWithInitializer(const std::shared_ptr request, std::shared_ptr response); + + + +void setPublishPoint(const std::shared_ptr request, + std::shared_ptr response); + + +void updateVariance(); +void updateTime(); +void updatePose(); +void updateGridMap(); +void publishStatistics(); + +void publishNormalAsArrow(const grid_map::GridMap& map) const; +void initializeWithTF(); +void publishMapToOdom(double error); +void publishMapOfIndex(int index); +visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; + + rclcpp::Node::SharedPtr node_; + // image_transport::ImageTransport it_; + std::vector::SharedPtr> pointcloudSubs_; + + // std::vector::SharedPtr> pointcloudtransportSubs_; + std::vector pointcloudtransportSubs_; + + std::vector::SharedPtr> imageSubs_; + std::vector::SharedPtr> cameraInfoSubs_; + std::vector::SharedPtr> channelInfoSubs_; + + + // std::vector imageSubs_; + // std::vector cameraInfoSubs_; + // std::vector channelInfoSubs_; + // std::vector cameraSyncs_; + // std::vector cameraChannelSyncs_; + // std::vector pointCloudSyncs_; + std::vector::SharedPtr> mapPubs_; + + + std::shared_ptr tfBroadcaster_; + std::shared_ptr tfListener_; + std::shared_ptr tfBuffer_; + + + rclcpp::Publisher::SharedPtr alivePub_; + rclcpp::Publisher::SharedPtr pointPub_; + rclcpp::Publisher::SharedPtr normalPub_; + rclcpp::Publisher::SharedPtr statisticsPub_; + rclcpp::Service::SharedPtr rawSubmapService_; + rclcpp::Service::SharedPtr clearMapService_; + rclcpp::Service::SharedPtr clearMapWithInitializerService_; + rclcpp::Service::SharedPtr initializeMapService_; + rclcpp::Service::SharedPtr setPublishPointService_; + rclcpp::Service::SharedPtr checkSafetyService_; + rclcpp::TimerBase::SharedPtr updateVarianceTimer_; + rclcpp::TimerBase::SharedPtr updateTimeTimer_; + rclcpp::TimerBase::SharedPtr updatePoseTimer_; + rclcpp::TimerBase::SharedPtr updateGridMapTimer_; + rclcpp::TimerBase::SharedPtr publishStatisticsTimer_; + rclcpp::Time lastStatisticsPublishedTime_; + + + std::shared_ptr map_; + // ElevationMappingWrapper map_; std::string mapFrameId_; std::string correctedMapFrameId_; std::string baseFrameId_; - // map topics info - std::vector> map_topics_; - std::vector> map_layers_; - std::vector> map_basic_layers_; - std::set map_layers_all_; - std::set map_layers_sync_; - std::vector map_fps_; - std::set map_fps_unique_; - std::vector mapTimers_; - std::map> channels_; - - std::vector initialize_frame_id_; - std::vector initialize_tf_offset_; - std::string initializeMethod_; - - Eigen::Vector3d lowpassPosition_; - Eigen::Vector4d lowpassOrientation_; - - std::mutex mapMutex_; // protects gridMap_ - grid_map::GridMap gridMap_; - std::atomic_bool isGridmapUpdated_; // needs to be atomic (read is not protected by mapMutex_) - - std::mutex errorMutex_; // protects positionError_, and orientationError_ - double positionError_; - double orientationError_; - - double positionAlpha_; - double orientationAlpha_; - - double recordableFps_; - std::atomic_bool enablePointCloudPublishing_; - bool enableNormalArrowPublishing_; - bool enableDriftCorrectedTFPublishing_; - bool useInitializerAtStart_; - double initializeTfGridSize_; - bool alwaysClearWithInitializer_; - std::atomic_int pointCloudProcessCounter_; + + // map topics info + std::vector> map_topics_; + std::vector> map_layers_; + std::vector> map_basic_layers_; + std::set map_layers_all_; + std::set map_layers_sync_; + std::vector map_fps_; + std::set map_fps_unique_; + std::vector mapTimers_; + std::map> channels_; + + std::vector initialize_frame_id_; + std::vector initialize_tf_offset_; + std::string initializeMethod_; + + Eigen::Vector3d lowpassPosition_; + Eigen::Vector4d lowpassOrientation_; + + std::mutex mapMutex_; // protects gridMap_ + grid_map::GridMap gridMap_; + std::atomic_bool isGridmapUpdated_; // needs to be atomic (read is not protected by mapMutex_) + + std::mutex errorMutex_; // protects positionError_, and orientationError_ + double positionError_; + double orientationError_; + + double positionAlpha_; + double orientationAlpha_; + double voxel_filter_size_; + pcl::VoxelGrid voxel_filter; + + double recordableFps_; + std::atomic_bool enablePointCloudPublishing_; + bool enableNormalArrowPublishing_; + bool enableDriftCorrectedTFPublishing_; + bool useInitializerAtStart_; + double initializeTfGridSize_; + bool alwaysClearWithInitializer_; + std::atomic_int pointCloudProcessCounter_; + + std::map> imageInfoReady_; + std::map> imageChannelReady_; }; } // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index 712ed10a..f0090254 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -15,16 +15,17 @@ #include // everything needed for embedding #include -// ROS -#include -#include -#include -#include -#include +// ROS2 +#include +#include +#include +#include +#include +#include // Grid Map -#include -#include +#include +#include #include // PCL @@ -36,15 +37,17 @@ namespace py = pybind11; namespace elevation_mapping_cupy { +std::vector extract_unique_names(const std::map& subscriber_params); + class ElevationMappingWrapper { public: using RowMatrixXd = Eigen::Matrix; using RowMatrixXf = Eigen::Matrix; using ColMatrixXf = Eigen::Matrix; - ElevationMappingWrapper(); + ElevationMappingWrapper(); - void initialize(ros::NodeHandle& nh); + void initialize(const std::shared_ptr& node); void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); @@ -62,9 +65,12 @@ class ElevationMappingWrapper { double get_additive_mean_error(); void initializeWithPoints(std::vector& points, std::string method); void addNormalColorLayer(grid_map::GridMap& map); - - private: - void setParameters(ros::NodeHandle& nh); + + private: + + std::shared_ptr node_; + void setParameters(); + py::object map_; py::object param_; double resolution_; diff --git a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py index 9a979b45..03d18530 100644 --- a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py +++ b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py @@ -2,13 +2,13 @@ from launch_ros.actions import Node from launch.substitutions import PathJoinSubstitution from ament_index_python.packages import get_package_share_directory - +import launch_ros.actions def generate_launch_description(): elevation_mapping_cupy_dir = get_package_share_directory('elevation_mapping_cupy') return LaunchDescription([ - # Elevation Mapping Node + launch_ros.actions.SetParameter(name='use_sim_time', value=True), Node( package='elevation_mapping_cupy', executable='elevation_mapping_node', diff --git a/elevation_mapping_cupy/launch/elevation_mapping_turtle.launch.py b/elevation_mapping_cupy/launch/elevation_mapping_turtle.launch.py index 7c6b06c3..92ac9d72 100644 --- a/elevation_mapping_cupy/launch/elevation_mapping_turtle.launch.py +++ b/elevation_mapping_cupy/launch/elevation_mapping_turtle.launch.py @@ -3,7 +3,9 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.descriptions import ParameterFile def generate_launch_description(): @@ -36,6 +38,13 @@ def generate_launch_description(): ) rviz_config = LaunchConfiguration('rviz_config') + use_python_node_arg = DeclareLaunchArgument( + 'use_python_node', + default_value='false', + description='Use the Python node if true' + ) + use_python_node = LaunchConfiguration('use_python_node') + # RViz Node rviz_node = Node( package='rviz2', @@ -45,21 +54,39 @@ def generate_launch_description(): parameters=[{'use_sim_time': use_sim_time}], output='screen' ) + elevation_mapping_node_py = Node( + package='elevation_mapping_cupy', + executable='elevation_mapping_node.py', + name='elevation_mapping_node', + output='screen', + parameters=[ + ParameterFile(core_param_path, allow_substs=True), + turtle_param_path, + {'use_sim_time': use_sim_time} + ], + condition=IfCondition(use_python_node) + # condition=IfCondition(PythonExpression(use_python_node)) + ) + elevation_mapping_node = Node( package='elevation_mapping_cupy', executable='elevation_mapping_node', name='elevation_mapping_node', output='screen', parameters=[ - core_param_path, + ParameterFile(core_param_path, allow_substs=True), turtle_param_path, {'use_sim_time': use_sim_time} - ] - ) + ], + condition=UnlessCondition(use_python_node) + # condition=UnlessCondition(PythonExpression(use_python_node)) + ) return LaunchDescription([ use_sim_time_arg, rviz_config_arg, + use_python_node_arg, + elevation_mapping_node_py, elevation_mapping_node, rviz_node ]) diff --git a/elevation_mapping_cupy/package.xml b/elevation_mapping_cupy/package.xml index e1bad0c5..f6880b52 100644 --- a/elevation_mapping_cupy/package.xml +++ b/elevation_mapping_cupy/package.xml @@ -1,47 +1,79 @@ - elevation_mapping_cupy - 2.0.0 - Elevation mapping on GPU - Your Name - MIT - - - ament_python - ament_cmake - - - rclpy - std_msgs - sensor_msgs - geometry_msgs - elevation_map_msgs - grid_map_msgs - grid_map_ros - image_transport - pcl_ros - pybind11 - ros2_numpy - tf2_ros - message_filters - cv_bridge - ament_index_python - tf_transformations - - - rviz2 - gazebo_ros - robot_state_publisher - opencv-python - - - ament_python - ament_python - - - ament_python - - - - https://github.com/yourrepo/elevation_mapping_cupy - + elevation_mapping_cupy + 2.0.0 + Elevation mapping on GPU + Takahiro Miki + MIT + + + ament_cmake + ament_cmake_python + + + + + + rclpy + + rclcpp + std_msgs + std_srvs + sensor_msgs + geometry_msgs + + point_cloud_transport + message_filters + elevation_map_msgs + grid_map_msgs + grid_map_ros + image_transport + pcl_ros + pybind11-dev + ros2_numpy + python3-scipy + + numpy_lessthan_2 + tf2_ros + cv_bridge + ament_index_python + tf_transformations + + + + rviz2 + + robot_state_publisher + python3-opencv + cupy-cuda12x + simple-parsing + python3-shapely + python3-ruamel.yaml + boost + python3-transforms3d + + + gazebo_ros_pkgs + turtlebot3_msgs + turtlebot3_teleop + camera_info_manager + camera_calibration_parsers + + + + + + + + + ament_lint_auto + ament_lint_common + + + + ament_cmake + + + + + \ No newline at end of file diff --git a/elevation_mapping_cupy/rviz/turtle_sim_laser.rviz b/elevation_mapping_cupy/rviz/turtle_sim_laser.rviz index 7b97d78c..8f247cfd 100644 --- a/elevation_mapping_cupy/rviz/turtle_sim_laser.rviz +++ b/elevation_mapping_cupy/rviz/turtle_sim_laser.rviz @@ -6,12 +6,8 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /PointCloud21 - - /RobotModel1 - - /TF1 - - /GridMap1 Splitter Ratio: 0.5 - Tree Height: 272 + Tree Height: 336 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -23,6 +19,7 @@ Panels: - Class: rviz_common/Views Expanded: - /Current View1 + - /Current View1/Focal Point1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time @@ -247,8 +244,9 @@ Visualization Manager: Value: /intel_realsense_r200_rgb/image_raw Value: false Visibility: - "": true Grid: true + GridMap: true + GridMapFiltered: true Image: true PointCloud2: true RobotModel: true @@ -256,7 +254,7 @@ Visualization Manager: Value: true Zoom Factor: 1 - Class: rviz_default_plugins/Image - Enabled: false + Enabled: true Max Value: 1 Median window: 5 Min Value: 0 @@ -268,7 +266,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /intel_realsense_r200_depth/image_raw - Value: false + Value: true - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -287,8 +285,8 @@ Visualization Manager: Autocompute Intensity Bounds: true Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 - Color Layer: elevation - Color Transformer: IntensityLayer + Color Layer: rgb + Color Transformer: ColorLayer Enabled: true Height Layer: elevation Height Transformer: GridMapLayer @@ -306,13 +304,39 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /elevation_mapping/elevation_map_raw + Value: /elevation_mapping_node/elevation_map_raw Use Rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: IntensityLayer + Enabled: false + Height Layer: smooth + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMapFiltered + Show Grid Lines: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_mapping_node/elevation_map_filter + Use Rainbow: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_footprint + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -354,38 +378,38 @@ Visualization Manager: Value: true Views: Current: - Class: rviz_default_plugins/Orbit - Distance: 6.990833282470703 + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 3.38887095451355 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.13978521525859833 - Y: 0.045054733753204346 - Z: 0.2562030851840973 + X: -0.7572358250617981 + Y: -0.10913658142089844 + Z: -0.018636569380760193 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3597964644432068 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.0935845375061035 + Pitch: 0.6897954940795898 + Target Frame: base_link + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.08858585357666 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 846 + Height: 954 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000199000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000136000000f70000002800fffffffb0000000a0049006d0061006700650000000157000000d60000002800fffffffb0000000a0049006d00610067006501000001da000001150000002800ffffff000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000025f00000320fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001d9000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000136000000f70000002800fffffffb0000000a0049006d0061006700650000000157000000d60000002800fffffffb0000000a0049006d006100670065010000021a000001410000002800ffffff000000010000010f00000320fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000320000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004060000032000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -394,6 +418,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 - X: 60 - Y: 169 + Width: 1920 + X: 1920 + Y: 27 diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_ros.py b/elevation_mapping_cupy/scripts/elevation_mapping_node.py old mode 100644 new mode 100755 similarity index 93% rename from elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_ros.py rename to elevation_mapping_cupy/scripts/elevation_mapping_node.py index 83f19ad4..50915c91 --- a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_ros.py +++ b/elevation_mapping_cupy/scripts/elevation_mapping_node.py @@ -5,6 +5,7 @@ import rclpy from rclpy.node import Node +from rclpy.qos import QoSPresetProfiles from ament_index_python.packages import get_package_share_directory import ros2_numpy as rnp from sensor_msgs.msg import PointCloud2, Image, CameraInfo @@ -232,12 +233,15 @@ def register_subscribers(self) -> None: image_subs[key] = image_sync elif data_type == "pointcloud": topic_name = config.get("topic_name", "/pointcloud") - qos_profile = rclpy.qos.QoSProfile( - depth=10, - reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, - durability=rclpy.qos.DurabilityPolicy.VOLATILE, - history=rclpy.qos.HistoryPolicy.KEEP_LAST - ) + # qos_profile = rclpy.qos.QoSProfile( + # depth=10, + # reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, + # durability=rclpy.qos.DurabilityPolicy.VOLATILE, + # history=rclpy.qos.HistoryPolicy.KEEP_LAST + # ) + # qos_profile = QoSPresetProfiles.get_from_short_key("sensor_data") + # qos_profile = rclpy.qos.QoSProfile(depth=10) + qos_profile = 10 subscription = self.create_subscription( PointCloud2, topic_name, @@ -355,12 +359,14 @@ def image_callback(self, camera_msg: Image, camera_info_msg: CameraInfo, sub_key def pointcloud_callback(self, msg: PointCloud2, sub_key: str) -> None: self._last_t = msg.header.stamp - channels = ["x", "y", "z"] + self.param.subscriber_cfg[sub_key].get("channels", []) + # self.get_logger().info(f"Received pointcloud with {msg.width} points") + additional_channels = self.param.subscriber_cfg[sub_key].get("channels", []) + channels = ["x", "y", "z"] + additional_channels try: points = rnp.numpify(msg) except: return - if points['xyz'].size == 0: + if points['x'].size == 0: return frame_sensor_id = msg.header.frame_id transform_sensor_to_map = self.safe_lookup_transform( @@ -372,7 +378,16 @@ def pointcloud_callback(self, msg: PointCloud2, sub_key: str) -> None: q = transform_sensor_to_map.transform.rotation t_np = np.array([t.x, t.y, t.z], dtype=np.float32) R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3].astype(np.float32) - self._map.input_pointcloud(points['xyz'], channels, R, t_np, 0, 0) + pts = rnp.point_cloud2.get_xyz_points(points) + # TODO: This is probably expensive. Consider modifying rnp or input_pointcloud() + # Append additional channels to pts + for channel in additional_channels: + if channel in points.dtype.names: + data = points[channel].flatten() + if data.ndim == 1: + data = data[:, np.newaxis] + pts = np.hstack((pts, data)) + self._map.input_pointcloud(pts, channels, R, t_np, 0, 0) self._pointcloud_process_counter += 1 def pose_update(self) -> None: diff --git a/elevation_mapping_cupy/setup.py b/elevation_mapping_cupy/setup.py index 493eef6c..6cd48a96 100644 --- a/elevation_mapping_cupy/setup.py +++ b/elevation_mapping_cupy/setup.py @@ -19,10 +19,11 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'elevation_mapping_node = elevation_mapping_cupy.elevation_mapping_ros:main', + 'elevation_mapping_node.py = elevation_mapping_cupy.elevation_mapping_node:main', ], }, data_files=[ + ('share/ament_index/resource_index/packages',['resource/' + package_name]), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), *[(os.path.join('share', package_name, os.path.dirname(yaml_file)), [yaml_file]) for yaml_file in glob('config/**/*.yaml', recursive=True)], # also the .*dat files diff --git a/elevation_mapping_cupy/src/elevation_mapping_node.cpp b/elevation_mapping_cupy/src/elevation_mapping_node.cpp index 3d7a778f..131e25cc 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_node.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_node.cpp @@ -6,22 +6,26 @@ // Pybind #include // everything needed for embedding -// ROS -#include - +// ROS2 +#include #include "elevation_mapping_cupy/elevation_mapping_ros.hpp" int main(int argc, char** argv) { - ros::init(argc, argv, "elevation_mapping"); - ros::NodeHandle nh("~"); + rclcpp::init(argc, argv); + // Allow declaring parameters from the yaml files + rclcpp::NodeOptions options; + options.automatically_declare_parameters_from_overrides(true); + options.allow_undeclared_parameters(true); + py::scoped_interpreter guard{}; // start the interpreter and keep it alive - elevation_mapping_cupy::ElevationMappingNode mapNode(nh); + auto mapNode = std::make_shared(options); py::gil_scoped_release release; + - // Spin - ros::AsyncSpinner spinner(1); // Use n threads - spinner.start(); - ros::waitForShutdown(); + // TODO: Create a multi-threaded executor + rclcpp::spin(mapNode); + rclcpp::shutdown(); return 0; } + diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 2fbc3128..9d18dc4e 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -3,25 +3,29 @@ // Licensed under the MIT license. See LICENSE file in the project root for details. // + #include "elevation_mapping_cupy/elevation_mapping_ros.hpp" // Pybind #include -// ROS -#include -#include -#include +// ROS2 +#include +#include +#include // PCL #include -#include +#include namespace elevation_mapping_cupy { -ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) - : it_(nh), + +ElevationMappingNode::ElevationMappingNode(const rclcpp::NodeOptions& options) + : Node("elevation_mapping_node", options), + node_(std::shared_ptr(this, [](auto *) {})), + // it_(node_), lowpassPosition_(0, 0, 0), lowpassOrientation_(0, 0, 0, 1), positionError_(0), @@ -29,208 +33,306 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) positionAlpha_(0.1), orientationAlpha_(0.1), enablePointCloudPublishing_(false), - isGridmapUpdated_(false) { - nh_ = nh; + isGridmapUpdated_(false){ + + RCLCPP_INFO(this->get_logger(), "Initializing ElevationMappingNode..."); + tfBroadcaster_ = std::make_shared(*this);// ROS2构造TransformBroadcaster + tfBuffer_ = std::make_shared(this->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + + std::string pose_topic, map_frame; - XmlRpc::XmlRpcValue publishers; - XmlRpc::XmlRpcValue subscribers; std::vector map_topics; double recordableFps, updateVarianceFps, timeInterval, updatePoseFps, updateGridMapFps, publishStatisticsFps; bool enablePointCloudPublishing(false); - - // Read parameters - nh.getParam("subscribers", subscribers); - nh.getParam("publishers", publishers); - if (!subscribers.valid()) { - ROS_FATAL("There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); - } - if (!publishers.valid()) { - ROS_FATAL("There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit"); - } - nh.param>("initialize_frame_id", initialize_frame_id_, {"base"}); - nh.param>("initialize_tf_offset", initialize_tf_offset_, {0.0}); - nh.param("pose_topic", pose_topic, "pose"); - nh.param("map_frame", mapFrameId_, "map"); - nh.param("base_frame", baseFrameId_, "base"); - nh.param("corrected_map_frame", correctedMapFrameId_, "corrected_map"); - nh.param("initialize_method", initializeMethod_, "cubic"); - nh.param("position_lowpass_alpha", positionAlpha_, 0.2); - nh.param("orientation_lowpass_alpha", orientationAlpha_, 0.2); - nh.param("recordable_fps", recordableFps, 3.0); - nh.param("update_variance_fps", updateVarianceFps, 1.0); - nh.param("time_interval", timeInterval, 0.1); - nh.param("update_pose_fps", updatePoseFps, 10.0); - nh.param("initialize_tf_grid_size", initializeTfGridSize_, 0.5); - nh.param("map_acquire_fps", updateGridMapFps, 5.0); - nh.param("publish_statistics_fps", publishStatisticsFps, 1.0); - nh.param("enable_pointcloud_publishing", enablePointCloudPublishing, false); - 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); + + py::gil_scoped_acquire acquire; + + this->get_parameter("initialize_frame_id", initialize_frame_id_); + this->get_parameter("initialize_tf_offset", initialize_tf_offset_); + this->get_parameter("pose_topic", pose_topic); + this->get_parameter("map_frame", mapFrameId_); + this->get_parameter("base_frame", baseFrameId_); + this->get_parameter("corrected_map_frame", correctedMapFrameId_); + this->get_parameter("initialize_method", initializeMethod_); + this->get_parameter("position_lowpass_alpha", positionAlpha_); + this->get_parameter("orientation_lowpass_alpha", orientationAlpha_); + this->get_parameter("recordable_fps", recordableFps); + this->get_parameter("update_variance_fps", updateVarianceFps); + this->get_parameter("time_interval", timeInterval); + this->get_parameter("update_pose_fps", updatePoseFps); + this->get_parameter("initialize_tf_grid_size", initializeTfGridSize_); + this->get_parameter("map_acquire_fps", updateGridMapFps); + this->get_parameter("publish_statistics_fps", publishStatisticsFps); + this->get_parameter("enable_pointcloud_publishing", enablePointCloudPublishing); + this->get_parameter("enable_normal_arrow_publishing", enableNormalArrowPublishing_); + this->get_parameter("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_); + this->get_parameter("use_initializer_at_start", useInitializerAtStart_); + this->get_parameter("always_clear_with_initializer", alwaysClearWithInitializer_); + this->get_parameter("voxel_filter_size", voxel_filter_size_); + + RCLCPP_INFO(this->get_logger(), "initialize_frame_id: %s", initialize_frame_id_.empty() ? "[]" : initialize_frame_id_[0].c_str()); + RCLCPP_INFO(this->get_logger(), "initialize_tf_offset: [%f, %f, %f, %f]", initialize_tf_offset_[0], initialize_tf_offset_[1], initialize_tf_offset_[2], initialize_tf_offset_[3]); + RCLCPP_INFO(this->get_logger(), "pose_topic: %s", pose_topic.c_str()); + RCLCPP_INFO(this->get_logger(), "map_frame: %s", mapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "base_frame: %s", baseFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "corrected_map_frame: %s", correctedMapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "initialize_method: %s", initializeMethod_.c_str()); + RCLCPP_INFO(this->get_logger(), "position_lowpass_alpha: %f", positionAlpha_); + RCLCPP_INFO(this->get_logger(), "orientation_lowpass_alpha: %f", orientationAlpha_); + RCLCPP_INFO(this->get_logger(), "recordable_fps: %f", recordableFps); + RCLCPP_INFO(this->get_logger(), "update_variance_fps: %f", updateVarianceFps); + RCLCPP_INFO(this->get_logger(), "time_interval: %f", timeInterval); + RCLCPP_INFO(this->get_logger(), "update_pose_fps: %f", updatePoseFps); + RCLCPP_INFO(this->get_logger(), "initialize_tf_grid_size: %f", initializeTfGridSize_); + RCLCPP_INFO(this->get_logger(), "map_acquire_fps: %f", updateGridMapFps); + RCLCPP_INFO(this->get_logger(), "publish_statistics_fps: %f", publishStatisticsFps); + RCLCPP_INFO(this->get_logger(), "enable_pointcloud_publishing: %s", enablePointCloudPublishing ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "enable_normal_arrow_publishing: %s", enableNormalArrowPublishing_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "enable_drift_corrected_TF_publishing: %s", enableDriftCorrectedTFPublishing_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "use_initializer_at_start: %s", useInitializerAtStart_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "always_clear_with_initializer: %s", alwaysClearWithInitializer_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "voxel_filter_size: %f", voxel_filter_size_); enablePointCloudPublishing_ = enablePointCloudPublishing; + + map_ = std::make_shared(); + map_->initialize(node_); - // Iterate all the subscribers - // here we have to remove all the stuff - for (auto& subscriber : subscribers) { - std::string key = subscriber.first; - auto type = static_cast(subscriber.second["data_type"]); - - // Initialize subscribers depending on the type - if (type == "pointcloud") { - std::string pointcloud_topic = subscriber.second["topic_name"]; - channels_[key].push_back("x"); - channels_[key].push_back("y"); - channels_[key].push_back("z"); - boost::function f = boost::bind(&ElevationMappingNode::pointcloudCallback, this, _1, key); - ros::Subscriber sub = nh_.subscribe(pointcloud_topic, 1, f); - pointcloudSubs_.push_back(sub); - ROS_INFO_STREAM("Subscribed to PointCloud2 topic: " << pointcloud_topic); - } - else if (type == "image") { - std::string camera_topic = subscriber.second["topic_name"]; - std::string info_topic = subscriber.second["camera_info_topic_name"]; - - // Handle compressed images with transport hints - // We obtain the hint from the last part of the topic name - std::string transport_hint = "compressed"; - std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name - if (ind != std::string::npos) { - transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part - camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic - } else { - transport_hint = "raw"; // In the default case we assume raw topic - } - // Setup subscriber - const auto hint = image_transport::TransportHints(transport_hint, ros::TransportHints(), ros::NodeHandle(camera_topic)); - ImageSubscriberPtr image_sub = std::make_shared(); - image_sub->subscribe(it_, camera_topic, 1, hint); - imageSubs_.push_back(image_sub); - - CameraInfoSubscriberPtr cam_info_sub = std::make_shared(); - cam_info_sub->subscribe(nh_, info_topic, 1); - cameraInfoSubs_.push_back(cam_info_sub); - - std::string channel_info_topic; - // If there is channel info topic setting, we use it. - if (subscriber.second.hasMember("channel_info_topic_name")) { - std::string channel_info_topic = subscriber.second["channel_info_topic_name"]; - ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(); - channel_info_sub->subscribe(nh_, channel_info_topic, 1); - channelInfoSubs_.push_back(channel_info_sub); - CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); - sync->registerCallback(boost::bind(&ElevationMappingNode::imageChannelCallback, this, _1, _2, _3)); - cameraChannelSyncs_.push_back(sync); - ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ", Channel info topic: " << channel_info_topic); - } - else { - // If there is channels setting, we use it. Otherwise, we use rgb as default. - if (subscriber.second.hasMember("channels")) { - const auto& channels = subscriber.second["channels"]; - for (int32_t i = 0; i < channels.size(); ++i) { - auto elem = static_cast(channels[i]); - channels_[key].push_back(elem); + + std::map subscriber_params, publisher_params; + if (!this->get_parameters("subscribers", subscriber_params)) { + RCLCPP_FATAL(this->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); + rclcpp::shutdown(); + } + if (!this->get_parameters("publishers", publisher_params)) { + RCLCPP_FATAL(this->get_logger(), "There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit"); + rclcpp::shutdown(); + } + + + auto unique_sub_names = extract_unique_names(subscriber_params); + for (const auto& sub_name : unique_sub_names) { + std::string data_type; + if(this->get_parameter("subscribers." + sub_name + ".data_type", data_type)){ + // image + if(data_type == "image"){ + std::string camera_topic; + std::string info_topic; + this->get_parameter("subscribers." + sub_name + ".topic_name", camera_topic); + this->get_parameter("subscribers." + sub_name + ".info_name", info_topic); + RCLCPP_INFO(this->get_logger(), "camera_topic %s: %s", sub_name.c_str(), camera_topic.c_str()); + RCLCPP_INFO(this->get_logger(), "info_name %s: %s", sub_name.c_str(), info_topic.c_str()); + + // std::string transport_hint = "compressed"; + // std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name + // if (ind != std::string::npos) { + // transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part + // camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic + // } else { + // transport_hint = "raw"; // In the default case we assume raw topic + // } + + std::string key = sub_name; + + + sensor_msgs::msg::CameraInfo img_info; + elevation_map_msgs::msg::ChannelInfo channel_info; + imageInfoReady_[key] = std::make_pair(img_info, false); + imageChannelReady_[key] = std::make_pair(channel_info, false); + // Image subscriber init + + rmw_qos_profile_t sensor_qos_profile = rmw_qos_profile_sensor_data; + auto sensor_qos = rclcpp::QoS(rclcpp::QoSInitialization(sensor_qos_profile.history, sensor_qos_profile.depth), sensor_qos_profile); + + auto img_callback = [this, key](const sensor_msgs::msg::Image::SharedPtr msg) { + this->imageCallback(msg, key); + }; + auto img_sub = this->create_subscription(camera_topic, sensor_qos, img_callback); + imageSubs_.push_back(img_sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s", camera_topic.c_str()); + // Camera Info subscriber init + auto img_info_callback = [this, key](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + this->imageInfoCallback(msg, key); + }; + auto img_info_sub = this->create_subscription(info_topic, sensor_qos, img_info_callback); + cameraInfoSubs_.push_back(img_info_sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to ImageInfo topic: %s", info_topic.c_str()); + + std::string channel_info_topic; + if (this->get_parameter("subscribers." + sub_name + ".channel_name", channel_info_topic)) { + // channel subscriber init + imageChannelReady_[key].second = false; + auto img_channel_callback = [this, key](const elevation_map_msgs::msg::ChannelInfo::SharedPtr msg) { + this->imageChannelCallback(msg, key); + }; + auto channel_info_sub = this->create_subscription(channel_info_topic, sensor_qos, img_channel_callback); + channelInfoSubs_.push_back(channel_info_sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to ChannelInfo topic: %s", channel_info_topic.c_str()); + }else{ + imageChannelReady_[key].second = true; + channels_[key].push_back("rgb"); + } + }else if(data_type == "pointcloud"){ + std::string pointcloud_topic; + this->get_parameter("subscribers." + sub_name + ".topic_name", pointcloud_topic); + std::string key = sub_name; + channels_[key].push_back("x"); + channels_[key].push_back("y"); + channels_[key].push_back("z"); + + // rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + // auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, qos_profile.depth), qos_profile); + + rmw_qos_profile_t sensor_qos_profile = rmw_qos_profile_sensor_data; + auto sensor_qos = rclcpp::QoS(rclcpp::QoSInitialization(sensor_qos_profile.history, sensor_qos_profile.depth), sensor_qos_profile); + + + // point_cloud_transport::Subscriber pct_sub = pct.subscribe( + // "pct/point_cloud", 100, + // [node](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) + // { + // RCLCPP_INFO_STREAM( + // node->get_logger(), + // "Message received, number of points is: " << msg->width * msg->height); + // }, {}); + // point_cloud_transport::Subscriber sub = pct.subscribe(pointcloud_topic, 100, callback, {}, transport_hint.get()) + + + auto callback_transport = [this, key](const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + this->pointcloudtransportCallback(msg, key); + }; + + // Create transport hints (e.g., "draco") + // auto transport_hint = std::make_shared("draco"); + + + // Use PointCloudTransport to create a subscriber + point_cloud_transport::PointCloudTransport pct(node_); + auto sub_transport = pct.subscribe(pointcloud_topic, 100, callback_transport); + + // Add the subscriber to the vector to manage its lifetime + pointcloudtransportSubs_.push_back(sub_transport); + + + // auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // this->pointcloudCallback(msg, key); + // }; + + // auto sub = this->create_subscription(pointcloud_topic, sensor_qos, callback); + + // pointcloudSubs_.push_back(sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to PointCloud2 topic: %s", pointcloud_topic.c_str()); } - } - else { - channels_[key].push_back("rgb"); - } - ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ". Channel info topic: " << (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")) : channel_info_topic)); - CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); - sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2, key)); - cameraSyncs_.push_back(sync); } - - - } else { - ROS_WARN_STREAM("Subscriber data_type [" << type << "] Not valid. Supported types: pointcloud, image"); - continue; } - } + + + + auto unique_pub_names = extract_unique_names(publisher_params); + + + std::string node_name = this->get_name(); + + for (const auto& pub_name : unique_pub_names) { + // Namespacing published topics under node_name + std::string topic_name = node_name + "/" + pub_name; + double fps; + std::vector layers_list; + std::vector basic_layers_list; + + this->get_parameter("publishers." + pub_name + ".layers", layers_list); + this->get_parameter("publishers." + pub_name + ".basic_layers", basic_layers_list); + this->get_parameter("publishers." + pub_name + ".fps", fps); + + if (fps > updateGridMapFps) { + RCLCPP_WARN( + this->get_logger(), + "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " + "fps.", + topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); + } - map_.initialize(nh_); - - // Register map publishers - for (auto itr = publishers.begin(); itr != publishers.end(); ++itr) { - // Parse params - std::string topic_name = itr->first; - std::vector layers_list; - std::vector basic_layers_list; - auto layers = itr->second["layers"]; - auto basic_layers = itr->second["basic_layers"]; - double fps = itr->second["fps"]; - - if (fps > updateGridMapFps) { - ROS_WARN( - "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " - "fps.", - topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); - } + // Make publishers + auto pub = this->create_publisher(topic_name, 1); + RCLCPP_INFO(this->get_logger(), "Publishing map to topic %s", topic_name.c_str()); + mapPubs_.push_back(pub); - for (int32_t i = 0; i < layers.size(); ++i) { - layers_list.push_back(static_cast(layers[i])); - } + // Register map layers + map_layers_.push_back(layers_list); + map_basic_layers_.push_back(basic_layers_list); - for (int32_t i = 0; i < basic_layers.size(); ++i) { - basic_layers_list.push_back(static_cast(basic_layers[i])); - } + // Register map fps + map_fps_.push_back(fps); + map_fps_unique_.insert(fps); - // Make publishers - ros::Publisher pub = nh_.advertise(topic_name, 1); - mapPubs_.push_back(pub); + } - // Register map layers - map_layers_.push_back(layers_list); - map_basic_layers_.push_back(basic_layers_list); - // Register map fps - map_fps_.push_back(fps); - map_fps_unique_.insert(fps); - } setupMapPublishers(); - pointPub_ = nh_.advertise("elevation_map_points", 1); - alivePub_ = nh_.advertise("alive", 1); - normalPub_ = nh_.advertise("normal", 1); - statisticsPub_ = nh_.advertise("statistics", 1); + pointPub_ = this->create_publisher("elevation_map_points", 1); + alivePub_ = this->create_publisher("alive", 1); + normalPub_ = this->create_publisher("normal", 1); + statisticsPub_ = this->create_publisher("statistics", 1); gridMap_.setFrameId(mapFrameId_); - rawSubmapService_ = nh_.advertiseService("get_raw_submap", &ElevationMappingNode::getSubmap, this); - clearMapService_ = nh_.advertiseService("clear_map", &ElevationMappingNode::clearMap, this); - initializeMapService_ = nh_.advertiseService("initialize", &ElevationMappingNode::initializeMap, this); - clearMapWithInitializerService_ = - nh_.advertiseService("clear_map_with_initializer", &ElevationMappingNode::clearMapWithInitializer, this); - setPublishPointService_ = nh_.advertiseService("set_publish_points", &ElevationMappingNode::setPublishPoint, this); - checkSafetyService_ = nh_.advertiseService("check_safety", &ElevationMappingNode::checkSafety, this); + +rawSubmapService_ = this->create_service( + "get_raw_submap", std::bind(&ElevationMappingNode::getSubmap, this, std::placeholders::_1, std::placeholders::_2)); + +clearMapService_ = this->create_service( + "clear_map", std::bind(&ElevationMappingNode::clearMap, this, std::placeholders::_1, std::placeholders::_2)); + +clearMapWithInitializerService_ = this->create_service( + "clear_map_with_initializer", std::bind(&ElevationMappingNode::clearMapWithInitializer, this, std::placeholders::_1, std::placeholders::_2)); + + +initializeMapService_ = this->create_service( + "initialize", std::bind(&ElevationMappingNode::initializeMap, this, std::placeholders::_1, std::placeholders::_2)); + +setPublishPointService_ = this->create_service( + "set_publish_points", std::bind(&ElevationMappingNode::setPublishPoint, this, std::placeholders::_1, std::placeholders::_2)); + +// checkSafetyService_ = this->create_service( +// "check_safety", std::bind(&ElevationMappingNode::checkSafety, this, std::placeholders::_1, std::placeholders::_2)); + if (updateVarianceFps > 0) { - double duration = 1.0 / (updateVarianceFps + 0.00001); - updateVarianceTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateVariance, this, false, true); - } - if (timeInterval > 0) { - double duration = timeInterval; - updateTimeTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateTime, this, false, true); - } - if (updatePoseFps > 0) { - double duration = 1.0 / (updatePoseFps + 0.00001); - updatePoseTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updatePose, this, false, true); - } - if (updateGridMapFps > 0) { - double duration = 1.0 / (updateGridMapFps + 0.00001); - updateGridMapTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateGridMap, this, false, true); + double duration = 1.0 / (updateVarianceFps + 0.00001); + updateVarianceTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updateVariance, this)); } - if (publishStatisticsFps > 0) { - double duration = 1.0 / (publishStatisticsFps + 0.00001); - publishStatisticsTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::publishStatistics, this, false, true); - } - lastStatisticsPublishedTime_ = ros::Time::now(); - ROS_INFO("[ElevationMappingCupy] finish initialization"); + if (timeInterval > 0) { + double duration = timeInterval; + updateTimeTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updateTime, this)); + } + if (updatePoseFps > 0) { + double duration = 1.0 / (updatePoseFps + 0.00001); + updatePoseTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updatePose, this)); + } + if (updateGridMapFps > 0) { + double duration = 1.0 / (updateGridMapFps + 0.00001); + updateGridMapTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updateGridMap, this)); + } + if (publishStatisticsFps > 0) { + double duration = 1.0 / (publishStatisticsFps + 0.00001); + publishStatisticsTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::publishStatistics, this)); + } + lastStatisticsPublishedTime_ = this->now(); + RCLCPP_INFO(this->get_logger(), "[ElevationMappingCupy] finish initialization"); } -// Setup map publishers + // namespace elevation_mapping_cupy + + +// // Setup map publishers void ElevationMappingNode::setupMapPublishers() { // Find the layers with highest fps. float max_fps = -1; @@ -256,35 +358,40 @@ void ElevationMappingNode::setupMapPublishers() { } // Callback funtion. // It publishes to specific topics. - auto cb = [this, indices](const ros::TimerEvent&) { + auto cb = [this, indices]() -> void { for (int i : indices) { - publishMapOfIndex(i); + publishMapOfIndex(i); } }; double duration = 1.0 / (fps + 0.00001); - mapTimers_.push_back(nh_.createTimer(ros::Duration(duration), cb)); + mapTimers_.push_back(this->create_wall_timer(std::chrono::duration(duration), cb)); } } + void ElevationMappingNode::publishMapOfIndex(int index) { // publish the map layers of index if (!isGridmapUpdated_) { return; } - grid_map_msgs::GridMap msg; + + + std::unique_ptr msg_ptr; + grid_map_msgs::msg::GridMap msg; + std::vector layers; - { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in + { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in // map_layers_all_ std::lock_guard lock(mapMutex_); for (const auto& layer : map_layers_[index]) { const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); if (is_layer_in_all && gridMap_.exists(layer)) { layers.push_back(layer); - } else if (map_.exists_layer(layer)) { + } else if (map_->exists_layer(layer)) { // if there are layers which is not in the syncing layer. ElevationMappingWrapper::RowMatrixXf map_data; - map_.get_layer_data(layer, map_data); + map_->get_layer_data(layer, map_data); gridMap_.add(layer, map_data); layers.push_back(layer); } @@ -293,22 +400,48 @@ void ElevationMappingNode::publishMapOfIndex(int index) { return; } - grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg); + msg_ptr = grid_map::GridMapRosConverter::toMessage(gridMap_, layers); + msg= *msg_ptr; } - + msg.basic_layers = map_basic_layers_[index]; - mapPubs_[index].publish(msg); + mapPubs_[index]->publish(msg); } -void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key) { +void ElevationMappingNode::imageInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr image_info, const std::string& key) { +imageInfoReady_[key] = std::make_pair(*image_info, true); + // Find and remove the subscription for this key + auto it = std::find_if(cameraInfoSubs_.begin(), cameraInfoSubs_.end(), + [key](const rclcpp::Subscription::SharedPtr& sub) { + return sub->get_topic_name() == key; + }); + if (it != cameraInfoSubs_.end()) { + cameraInfoSubs_.erase(it); + } + +} +void ElevationMappingNode::imageChannelCallback(const elevation_map_msgs::msg::ChannelInfo::SharedPtr channel_info, const std::string& key) { +imageChannelReady_[key] = std::make_pair(*channel_info, true); + auto it = std::find_if(channelInfoSubs_.begin(), channelInfoSubs_.end(), + [key](const rclcpp::Subscription::SharedPtr& sub) { + return sub->get_topic_name() == key; + }); + if (it != channelInfoSubs_.end()) { + channelInfoSubs_.erase(it); + } +} + + +void ElevationMappingNode::pointcloudtransportCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const std::string& key) { + // get channels - auto fields = cloud.fields; + auto fields = cloud->fields; std::vector channels; - for (int it = 0; it < fields.size(); it++) { - auto& field = fields[it]; - channels.push_back(field.name); + for (size_t it = 0; it < fields.size(); it++) { + auto& field = fields[it]; + channels.push_back(field.name); } inputPointCloud(cloud, channels); @@ -316,65 +449,93 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cl pointCloudProcessCounter_++; } -void ElevationMappingNode::inputPointCloud(const sensor_msgs::PointCloud2& cloud, - const std::vector& channels) { - auto start = ros::Time::now(); - auto* pcl_pc = new pcl::PCLPointCloud2; - pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc); - pcl_conversions::toPCL(cloud, *pcl_pc); - +void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key) { // get channels - auto fields = cloud.fields; - uint array_dim = channels.size(); + auto fields = cloud->fields; + std::vector channels; - RowMatrixXd points = RowMatrixXd(pcl_pc->width * pcl_pc->height, array_dim); + for (size_t it = 0; it < fields.size(); it++) { + auto& field = fields[it]; + channels.push_back(field.name); + } + inputPointCloud(cloud, channels); + + // This is used for publishing as statistics. + pointCloudProcessCounter_++; +} - for (unsigned int i = 0; i < pcl_pc->width * pcl_pc->height; ++i) { - for (unsigned int j = 0; j < channels.size(); ++j) { - float temp; - uint point_idx = i * pcl_pc->point_step + pcl_pc->fields[j].offset; - memcpy(&temp, &pcl_pc->data[point_idx], sizeof(float)); - points(i, j) = static_cast(temp); +void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, + const std::vector& channels) { + auto start = this->now(); + // auto* raw_pcl_pc = new pcl::PCLPointCloud2; + // pcl::PCLPointCloud2ConstPtr cloudPtr(raw_pcl_pc); + pcl::PCLPointCloud2::Ptr raw_pcl_pc(new pcl::PCLPointCloud2()); + pcl_conversions::toPCL(*cloud, *raw_pcl_pc); + + // apply the voxel filtering + pcl::PCLPointCloud2::Ptr pcl_pc (new pcl::PCLPointCloud2()); + // pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(raw_pcl_pc); + voxel_filter.setLeafSize(voxel_filter_size_,voxel_filter_size_,voxel_filter_size_); + voxel_filter.filter(*pcl_pc); + + RCLCPP_DEBUG(this->get_logger(), "Voxel grid filtered point cloud from %d points to %d points.", static_cast(raw_pcl_pc->width * raw_pcl_pc->height), static_cast(pcl_pc->width * pcl_pc->height)); + + // Get channels + auto fields = cloud->fields; + uint array_dim = channels.size(); + + RowMatrixXd points = RowMatrixXd(pcl_pc->width * pcl_pc->height, array_dim); + + for (unsigned int i = 0; i < pcl_pc->width * pcl_pc->height; ++i) { + for (unsigned int j = 0; j < channels.size(); ++j) { + float temp; + uint point_idx = i * pcl_pc->point_step + pcl_pc->fields[j].offset; + memcpy(&temp, &pcl_pc->data[point_idx], sizeof(float)); + points(i, j) = static_cast(temp); + } } - } - // get pose of sensor in map frame - tf::StampedTransform transformTf; - std::string sensorFrameId = cloud.header.frame_id; - auto timeStamp = cloud.header.stamp; - Eigen::Affine3d transformationSensorToMap; - try { - transformListener_.waitForTransform(mapFrameId_, sensorFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationSensorToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return; - } - double positionError{0.0}; - double orientationError{0.0}; - { - std::lock_guard lock(errorMutex_); - positionError = positionError_; - orientationError = orientationError_; - } - map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, - orientationError); + // Get pose of sensor in map frame + geometry_msgs::msg::TransformStamped transformStamped; + std::string sensorFrameId = cloud->header.frame_id; + auto timeStamp = cloud->header.stamp; + Eigen::Affine3d transformationSensorToMap; + try { + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, sensorFrameId, tf2::TimePointZero); + transformationSensorToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } - if (enableDriftCorrectedTFPublishing_) { - publishMapToOdom(map_.get_additive_mean_error()); - } + double positionError{0.0}; + double orientationError{0.0}; + { + std::lock_guard lock(errorMutex_); + positionError = positionError_; + orientationError = orientationError_; + } + + map_->input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, + orientationError); - ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), - (ros::Time::now() - start).toSec()); - ROS_DEBUG_THROTTLE(1.0, "positionError: %f ", positionError); - ROS_DEBUG_THROTTLE(1.0, "orientationError: %f ", orientationError); + if (enableDriftCorrectedTFPublishing_) { + publishMapToOdom(map_->get_additive_mean_error()); + } + RCLCPP_DEBUG(this->get_logger(), "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), + (this->now() - start).seconds()); + RCLCPP_DEBUG(this->get_logger(), "positionError: %f ", positionError); + RCLCPP_DEBUG(this->get_logger(), "orientationError: %f ", orientationError); } -void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const std::vector& channels) { + + + +void ElevationMappingNode::inputImage(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg, + const std::vector& channels) { // Get image cv::Mat image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; @@ -386,14 +547,14 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms } // Extract camera matrix - Eigen::Map> cameraMatrix(&camera_info_msg->K[0]); + Eigen::Map> cameraMatrix(&camera_info_msg->k[0]); // Extract distortion coefficients Eigen::VectorXd distortionCoeffs; - if (!camera_info_msg->D.empty()) { - distortionCoeffs = Eigen::Map(camera_info_msg->D.data(), camera_info_msg->D.size()); + if (!camera_info_msg->d.empty()) { + distortionCoeffs = Eigen::Map(camera_info_msg->d.data(), camera_info_msg->d.size()); } else { - ROS_WARN("Distortion coefficients are empty."); + RCLCPP_WARN(this->get_logger(), "Distortion coefficients are empty."); distortionCoeffs = Eigen::VectorXd::Zero(5); // return; } @@ -401,17 +562,16 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms // distortion model std::string distortion_model = camera_info_msg->distortion_model; - // Get pose of sensor in map frame - tf::StampedTransform transformTf; + // Get pose of sensor in map frame + geometry_msgs::msg::TransformStamped transformStamped; std::string sensorFrameId = image_msg->header.frame_id; auto timeStamp = image_msg->header.stamp; - Eigen::Affine3d transformationMapToSensor; + Eigen::Isometry3d transformationMapToSensor; try { - transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(sensorFrameId, mapFrameId_, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationMapToSensor); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + transformStamped = tfBuffer_->lookupTransform(sensorFrameId, mapFrameId_, tf2::TimePointZero); + transformationMapToSensor = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); return; } @@ -435,54 +595,71 @@ void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_ms } } if (total_channels != multichannel_image.size()) { - ROS_ERROR("Mismatch in the size of multichannel_image (%d), channels (%d). Please check the input.", multichannel_image.size(), channels.size()); - ROS_ERROR_STREAM("Current Channels: " << boost::algorithm::join(channels, ", ")); + RCLCPP_ERROR(this->get_logger(), "Mismatch in the size of multichannel_image (%d), channels (%d). Please check the input.", multichannel_image.size(), channels.size()); + for (const auto& channel : channels) { + RCLCPP_INFO(this->get_logger(), "Channel: %s", channel.c_str()); + } return; } // Pass image to pipeline - map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, + map_->input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, distortionCoeffs, distortion_model, image.rows, image.cols); } -void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const std::string& key) { - auto start = ros::Time::now(); - inputImage(image_msg, camera_info_msg, channels_[key]); - ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); -} -void ElevationMappingNode::imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg) { - auto start = ros::Time::now(); - // Default channels and fusion methods for image is rgb and image_color - std::vector channels; - channels = channel_info_msg->channels; - inputImage(image_msg, camera_info_msg, channels); - ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); + +void ElevationMappingNode::imageCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const std::string& key){ + auto start = this->now(); + + if (!imageInfoReady_[key].second){ + RCLCPP_WARN(this->get_logger(), "CameraInfo for key %s is not available yet.", key.c_str()); + return; + } + + + auto camera_info_msg = std::make_shared(imageInfoReady_[key].first); + if (std::find(channels_[key].begin(), channels_[key].end(), "rgb") != channels_[key].end()){ + inputImage(image_msg, camera_info_msg, channels_[key]); + } + else{ + if (!imageChannelReady_[key].second){ + RCLCPP_WARN(this->get_logger(), "ChannelInfo for key %s is not available yet.", key.c_str()); + return; + } + // Default channels and fusion methods for image is rgb and image_color + std::vector channels; + channels = imageChannelReady_[key].first.channels; + inputImage(image_msg, camera_info_msg, channels); + } + RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); } -void ElevationMappingNode::updatePose(const ros::TimerEvent&) { - tf::StampedTransform transformTf; - const auto& timeStamp = ros::Time::now(); + +void ElevationMappingNode::updatePose() { + geometry_msgs::msg::TransformStamped transformStamped; + const auto& timeStamp = this->now(); Eigen::Affine3d transformationBaseToMap; try { - transformListener_.waitForTransform(mapFrameId_, baseFrameId_, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, baseFrameId_, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, baseFrameId_, timeStamp, tf2::durationFromSec(1.0)); + transformationBaseToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); return; } // This is to check if the robot is moving. If the robot is not moving, drift compensation is disabled to avoid creating artifacts. - Eigen::Vector3d position(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); - map_.move_to(position, transformationBaseToMap.rotation().transpose()); - Eigen::Vector3d position3(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); - Eigen::Vector4d orientation(transformTf.getRotation().x(), transformTf.getRotation().y(), transformTf.getRotation().z(), - transformTf.getRotation().w()); + Eigen::Vector3d position(transformStamped.transform.translation.x, + transformStamped.transform.translation.y, + transformStamped.transform.translation.z); + map_->move_to(position, transformationBaseToMap.rotation().transpose()); + Eigen::Vector3d position3(transformStamped.transform.translation.x, + transformStamped.transform.translation.y, + transformStamped.transform.translation.z); + Eigen::Vector4d orientation(transformStamped.transform.rotation.x, + transformStamped.transform.rotation.y, + transformStamped.transform.rotation.z, + transformStamped.transform.rotation.w); lowpassPosition_ = positionAlpha_ * position3 + (1 - positionAlpha_) * lowpassPosition_; lowpassOrientation_ = orientationAlpha_ * orientation + (1 - orientationAlpha_) * lowpassOrientation_; { @@ -492,97 +669,118 @@ void ElevationMappingNode::updatePose(const ros::TimerEvent&) { } if (useInitializerAtStart_) { - ROS_INFO("Clearing map with initializer."); + RCLCPP_INFO(this->get_logger(), "Clearing map with initializer."); initializeWithTF(); useInitializerAtStart_ = false; } } void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const { - sensor_msgs::PointCloud2 msg; + sensor_msgs::msg::PointCloud2 msg; grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg); - pointPub_.publish(msg); + pointPub_->publish(msg); } -bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) { - std::string requestedFrameId = request.frame_id; + +bool ElevationMappingNode::getSubmap( + const std::shared_ptr request, + std::shared_ptr response) { + std::string requestedFrameId = request->frame_id; Eigen::Isometry3d transformationOdomToMap; - grid_map::Position requestedSubmapPosition(request.position_x, request.position_y); + geometry_msgs::msg::Pose pose; + grid_map::Position requestedSubmapPosition(request->position_x, request->position_y); + + if (requestedFrameId != mapFrameId_) { - tf::StampedTransform transformTf; - const auto& timeStamp = ros::Time::now(); + geometry_msgs::msg::TransformStamped transformStamped; + try { - transformListener_.waitForTransform(requestedFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(requestedFrameId, mapFrameId_, timeStamp, transformTf); - tf::poseTFToEigen(transformTf, transformationOdomToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + const auto& timeStamp = this->now(); + transformStamped = tfBuffer_->lookupTransform(requestedFrameId, mapFrameId_, timeStamp, tf2::durationFromSec(1.0)); + + + pose.position.x = transformStamped.transform.translation.x; + pose.position.y = transformStamped.transform.translation.y; + pose.position.z = transformStamped.transform.translation.z; + pose.orientation = transformStamped.transform.rotation; + + tf2::fromMsg(pose, transformationOdomToMap); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); return false; } - Eigen::Vector3d p(request.position_x, request.position_y, 0); + Eigen::Vector3d p(request->position_x, request->position_y, 0); Eigen::Vector3d mapP = transformationOdomToMap.inverse() * p; requestedSubmapPosition.x() = mapP.x(); requestedSubmapPosition.y() = mapP.y(); } - grid_map::Length requestedSubmapLength(request.length_x, request.length_y); - ROS_DEBUG("Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", requestedSubmapPosition.x(), requestedSubmapPosition.y(), - requestedSubmapLength(0), requestedSubmapLength(1)); + grid_map::Length requestedSubmapLength(request->length_x, request->length_y); + RCLCPP_DEBUG(this->get_logger(), "Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", + requestedSubmapPosition.x(), requestedSubmapPosition.y(), + requestedSubmapLength(0), requestedSubmapLength(1)); bool isSuccess; grid_map::Index index; grid_map::GridMap subMap; { std::lock_guard lock(mapMutex_); - subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, index, isSuccess); + subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, isSuccess); } const auto& length = subMap.getLength(); if (requestedFrameId != mapFrameId_) { subMap = subMap.getTransformedMap(transformationOdomToMap, "elevation", requestedFrameId); } - if (request.layers.empty()) { - grid_map::GridMapRosConverter::toMessage(subMap, response.map); + if (request->layers.empty()) { + auto msg_ptr = grid_map::GridMapRosConverter::toMessage(subMap); + response->map = *msg_ptr; } else { std::vector layers; - for (const auto& layer : request.layers) { + for (const auto& layer : request->layers) { layers.push_back(layer); } - grid_map::GridMapRosConverter::toMessage(subMap, layers, response.map); + auto msg_ptr = grid_map::GridMapRosConverter::toMessage(subMap, layers); + response->map = *msg_ptr; + } + return isSuccess; } -bool ElevationMappingNode::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { - ROS_INFO("Clearing map."); - map_.clear(); - if (alwaysClearWithInitializer_) { - initializeWithTF(); - } - return true; + + +void ElevationMappingNode::clearMap(const std::shared_ptr request, + std::shared_ptr response) { + + std::lock_guard lock(mapMutex_); + RCLCPP_INFO(this->get_logger(), "Clearing map."); + map_->clear(); + if (alwaysClearWithInitializer_) { + initializeWithTF(); + } } -bool ElevationMappingNode::clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { - ROS_INFO("Clearing map with initializer."); - map_.clear(); - initializeWithTF(); - return true; +void ElevationMappingNode::clearMapWithInitializer(const std::shared_ptr request, std::shared_ptr response){ + RCLCPP_INFO(this->get_logger(), "Clearing map with initializer"); + map_->clear(); + initializeWithTF(); + } void ElevationMappingNode::initializeWithTF() { std::vector points; - const auto& timeStamp = ros::Time::now(); + const auto& timeStamp = this->now(); int i = 0; Eigen::Vector3d p; for (const auto& frame_id : initialize_frame_id_) { // Get tf from map frame to tf frame Eigen::Affine3d transformationBaseToMap; - tf::StampedTransform transformTf; + geometry_msgs::msg::TransformStamped transformStamped; try { - transformListener_.waitForTransform(mapFrameId_, frame_id, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, frame_id, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, frame_id, timeStamp, tf2::durationFromSec(1.0)); + transformationBaseToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); return; } p = transformationBaseToMap.translation(); @@ -596,100 +794,103 @@ void ElevationMappingNode::initializeWithTF() { points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, -initializeTfGridSize_, 0)); points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, -initializeTfGridSize_, 0)); } - ROS_INFO_STREAM("Initializing map with points using " << initializeMethod_); - map_.initializeWithPoints(points, initializeMethod_); + RCLCPP_INFO(this->get_logger(), "Initializing map with points using %s", initializeMethod_.c_str()); + map_->initializeWithPoints(points, initializeMethod_); } -bool ElevationMappingNode::checkSafety(elevation_map_msgs::CheckSafety::Request& request, - elevation_map_msgs::CheckSafety::Response& response) { - for (const auto& polygonstamped : request.polygons) { - if (polygonstamped.polygon.points.empty()) { - continue; - } - std::vector polygon; - std::vector untraversable_polygon; - Eigen::Vector3d result; - result.setZero(); - const auto& polygonFrameId = polygonstamped.header.frame_id; - const auto& timeStamp = polygonstamped.header.stamp; - double polygon_z = polygonstamped.polygon.points[0].z; - - // Get tf from map frame to polygon frame - if (mapFrameId_ != polygonFrameId) { - Eigen::Affine3d transformationBaseToMap; - tf::StampedTransform transformTf; - try { - transformListener_.waitForTransform(mapFrameId_, polygonFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, polygonFrameId, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return false; - } - for (const auto& p : polygonstamped.polygon.points) { - const auto& pvector = Eigen::Vector3d(p.x, p.y, p.z); - const auto transformed_p = transformationBaseToMap * pvector; - polygon.emplace_back(Eigen::Vector2d(transformed_p.x(), transformed_p.y())); - } - } else { - for (const auto& p : polygonstamped.polygon.points) { - polygon.emplace_back(Eigen::Vector2d(p.x, p.y)); - } - } - - map_.get_polygon_traversability(polygon, result, untraversable_polygon); - - geometry_msgs::PolygonStamped untraversable_polygonstamped; - untraversable_polygonstamped.header.stamp = ros::Time::now(); - untraversable_polygonstamped.header.frame_id = mapFrameId_; - for (const auto& p : untraversable_polygon) { - geometry_msgs::Point32 point; - point.x = static_cast(p.x()); - point.y = static_cast(p.y()); - point.z = static_cast(polygon_z); - untraversable_polygonstamped.polygon.points.push_back(point); - } - // traversability_result; - response.is_safe.push_back(bool(result[0] > 0.5)); - response.traversability.push_back(result[1]); - response.untraversable_polygons.push_back(untraversable_polygonstamped); - } - return true; +// bool ElevationMappingNode::checkSafety(elevation_map_msgs::CheckSafety::Request& request, +// elevation_map_msgs::CheckSafety::Response& response) { +// for (const auto& polygonstamped : request.polygons) { +// if (polygonstamped.polygon.points.empty()) { +// continue; +// } +// std::vector polygon; +// std::vector untraversable_polygon; +// Eigen::Vector3d result; +// result.setZero(); +// const auto& polygonFrameId = polygonstamped.header.frame_id; +// const auto& timeStamp = polygonstamped.header.stamp; +// double polygon_z = polygonstamped.polygon.points[0].z; + +// // Get tf from map frame to polygon frame +// if (mapFrameId_ != polygonFrameId) { +// Eigen::Affine3d transformationBaseToMap; +// tf::StampedTransform transformTf; +// try { +// transformListener_.waitForTransform(mapFrameId_, polygonFrameId, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(mapFrameId_, polygonFrameId, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationBaseToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return false; +// } +// for (const auto& p : polygonstamped.polygon.points) { +// const auto& pvector = Eigen::Vector3d(p.x, p.y, p.z); +// const auto transformed_p = transformationBaseToMap * pvector; +// polygon.emplace_back(Eigen::Vector2d(transformed_p.x(), transformed_p.y())); +// } +// } else { +// for (const auto& p : polygonstamped.polygon.points) { +// polygon.emplace_back(Eigen::Vector2d(p.x, p.y)); +// } +// } + +// map_.get_polygon_traversability(polygon, result, untraversable_polygon); + +// geometry_msgs::PolygonStamped untraversable_polygonstamped; +// untraversable_polygonstamped.header.stamp = ros::Time::now(); +// untraversable_polygonstamped.header.frame_id = mapFrameId_; +// for (const auto& p : untraversable_polygon) { +// geometry_msgs::Point32 point; +// point.x = static_cast(p.x()); +// point.y = static_cast(p.y()); +// point.z = static_cast(polygon_z); +// untraversable_polygonstamped.polygon.points.push_back(point); +// } +// // traversability_result; +// response.is_safe.push_back(bool(result[0] > 0.5)); +// response.traversability.push_back(result[1]); +// response.untraversable_polygons.push_back(untraversable_polygonstamped); +// } +// return true; +// } + + + +void ElevationMappingNode::setPublishPoint(const std::shared_ptr request, + std::shared_ptr response) { + enablePointCloudPublishing_ = request->data; + response->success = true; } -bool ElevationMappingNode::setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { - enablePointCloudPublishing_ = request.data; - response.success = true; - return true; -} -void ElevationMappingNode::updateVariance(const ros::TimerEvent&) { - map_.update_variance(); +void ElevationMappingNode::updateVariance() { + map_->update_variance(); } -void ElevationMappingNode::updateTime(const ros::TimerEvent&) { - map_.update_time(); +void ElevationMappingNode::updateTime() { + map_->update_time(); } -void ElevationMappingNode::publishStatistics(const ros::TimerEvent&) { - ros::Time now = ros::Time::now(); - double dt = (now - lastStatisticsPublishedTime_).toSec(); +void ElevationMappingNode::publishStatistics() { + auto now = this->now(); + double dt = (now - lastStatisticsPublishedTime_).seconds(); lastStatisticsPublishedTime_ = now; - elevation_map_msgs::Statistics msg; + elevation_map_msgs::msg::Statistics msg; msg.header.stamp = now; if (dt > 0.0) { msg.pointcloud_process_fps = pointCloudProcessCounter_ / dt; } pointCloudProcessCounter_ = 0; - statisticsPub_.publish(msg); + statisticsPub_->publish(msg); } -void ElevationMappingNode::updateGridMap(const ros::TimerEvent&) { +void ElevationMappingNode::updateGridMap() { std::vector layers(map_layers_all_.begin(), map_layers_all_.end()); std::lock_guard lock(mapMutex_); - map_.get_grid_map(gridMap_, layers); - gridMap_.setTimestamp(ros::Time::now().toNSec()); - alivePub_.publish(std_msgs::Empty()); + map_->get_grid_map(gridMap_, layers); + gridMap_.setTimestamp(this->now().nanoseconds()); + alivePub_->publish(std_msgs::msg::Empty()); // Mostly debug purpose if (enablePointCloudPublishing_) { @@ -701,12 +902,12 @@ void ElevationMappingNode::updateGridMap(const ros::TimerEvent&) { isGridmapUpdated_ = true; } -bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request& request, - elevation_map_msgs::Initialize::Response& response) { +void ElevationMappingNode::initializeMap(const std::shared_ptr request, + std::shared_ptr response) { // If initialize method is points - if (request.type == request.POINTS) { + if (request->type == elevation_map_msgs::srv::Initialize::Request::POINTS) { std::vector points; - for (const auto& point : request.points) { + for (const auto& point : request->points) { const auto& pointFrameId = point.header.frame_id; const auto& timeStamp = point.header.stamp; const auto& pvector = Eigen::Vector3d(point.point.x, point.point.y, point.point.z); @@ -714,14 +915,14 @@ bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request // Get tf from map frame to points' frame if (mapFrameId_ != pointFrameId) { Eigen::Affine3d transformationBaseToMap; - tf::StampedTransform transformTf; + geometry_msgs::msg::TransformStamped transformStamped; try { - transformListener_.waitForTransform(mapFrameId_, pointFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, pointFrameId, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return false; + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, pointFrameId, timeStamp, tf2::durationFromSec(1.0)); + transformationBaseToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + response->success = false; + return; } const auto transformed_p = transformationBaseToMap * pvector; points.push_back(transformed_p); @@ -730,33 +931,32 @@ bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request } } std::string method; - switch (request.method) { - case request.NEAREST: + switch (request->method) { + case elevation_map_msgs::srv::Initialize::Request::NEAREST: method = "nearest"; break; - case request.LINEAR: + case elevation_map_msgs::srv::Initialize::Request::LINEAR: method = "linear"; break; - case request.CUBIC: + case elevation_map_msgs::srv::Initialize::Request::CUBIC: method = "cubic"; break; } - ROS_INFO_STREAM("Initializing map with points using " << method); - map_.initializeWithPoints(points, method); + RCLCPP_INFO(this->get_logger(), "Initializing map with points using %s", method.c_str()); + map_->initializeWithPoints(points, method); } - response.success = true; - return true; + response->success = true; } void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const { - auto startTime = ros::Time::now(); + auto startTime = this->now(); const auto& normalX = map["normal_x"]; const auto& normalY = map["normal_y"]; const auto& normalZ = map["normal_z"]; double scale = 0.1; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; // For each cell in map. for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { if (!map.isValid(*iterator, "elevation")) { @@ -773,19 +973,21 @@ void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) co } markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); } - normalPub_.publish(markerArray); - ROS_INFO_THROTTLE(1.0, "publish as normal in %f sec.", (ros::Time::now() - startTime).toSec()); + normalPub_->publish(markerArray); + } -visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, - const int id) const { - visualization_msgs::Marker marker; + + +visualization_msgs::msg::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, + const int id) const { + visualization_msgs::msg::Marker marker; marker.header.frame_id = mapFrameId_; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = this->now(); marker.ns = "normal"; marker.id = id; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; marker.points.resize(2); marker.points[0].x = start.x(); marker.points[0].y = start.y(); @@ -808,13 +1010,26 @@ visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen return marker; } + void ElevationMappingNode::publishMapToOdom(double error) { - tf::Transform transform; - transform.setOrigin(tf::Vector3(0.0, 0.0, error)); - tf::Quaternion q; + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = this->now(); + transform_stamped.header.frame_id = mapFrameId_; + transform_stamped.child_frame_id = correctedMapFrameId_; + transform_stamped.transform.translation.x = 0.0; + transform_stamped.transform.translation.y = 0.0; + transform_stamped.transform.translation.z = error; + + tf2::Quaternion q; q.setRPY(0, 0, 0); - transform.setRotation(q); - tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_)); + transform_stamped.transform.rotation.x = q.x(); + transform_stamped.transform.rotation.y = q.y(); + transform_stamped.transform.rotation.z = q.z(); + transform_stamped.transform.rotation.w = q.w(); + + tfBroadcaster_->sendTransform(transform_stamped); } + + } // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index 3980a8be..93beb0b4 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -12,162 +12,200 @@ #include // ROS -#include +#include +#include #include + + namespace elevation_mapping_cupy { -ElevationMappingWrapper::ElevationMappingWrapper() {} +std::vector extract_unique_names(const std::map& subscriber_params) { + std::set unique_names_set; + for (const auto& param : subscriber_params) { + std::size_t pos = param.first.find('.'); + if (pos != std::string::npos) { + std::string name = param.first.substr(0, pos); + unique_names_set.insert(name); + } + } + return std::vector(unique_names_set.begin(), unique_names_set.end()); +} -void ElevationMappingWrapper::initialize(ros::NodeHandle& nh) { + +ElevationMappingWrapper::ElevationMappingWrapper(){} + +void ElevationMappingWrapper::initialize(const std::shared_ptr& node){ + if (!node) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid node shared pointer"); + return; + } + node_ = node; // Add the elevation_mapping_cupy path to sys.path auto threading = py::module::import("threading"); py::gil_scoped_acquire acquire; - auto sys = py::module::import("sys"); - auto path = sys.attr("path"); - std::string module_path = ros::package::getPath("elevation_mapping_cupy"); - module_path = module_path + "/script"; - path.attr("insert")(0, module_path); auto elevation_mapping = py::module::import("elevation_mapping_cupy.elevation_mapping"); auto parameter = py::module::import("elevation_mapping_cupy.parameter"); - param_ = parameter.attr("Parameter")(); - setParameters(nh); + param_ = parameter.attr("Parameter")(); + setParameters(); map_ = elevation_mapping.attr("ElevationMap")(param_); + RCLCPP_INFO(node_->get_logger(), "ElevationMappingWrapper has been initialized"); + } -/** - * Load ros parameters into Parameter class. - * Search for the same name within the name space. - */ -void ElevationMappingWrapper::setParameters(ros::NodeHandle& nh) { +// /** +// * Load ros parameters into Parameter class. +// * Search for the same name within the name space. +// */ +void ElevationMappingWrapper::setParameters() { + // Get all parameters names and types. py::list paramNames = param_.attr("get_names")(); py::list paramTypes = param_.attr("get_types")(); py::gil_scoped_acquire acquire; - // Try to find the parameter in the ros parameter server. - // If there was a parameter, set it to the Parameter variable. - for (int i = 0; i < paramNames.size(); i++) { + // // Try to find the parameter in the ROS parameter server. + // // If there was a parameter, set it to the Parameter variable. + for (size_t i = 0; i < paramNames.size(); i++) { std::string type = py::cast(paramTypes[i]); std::string name = py::cast(paramNames[i]); + RCLCPP_INFO(node_->get_logger(), "type: %s, name %s", type.c_str(), name.c_str()); if (type == "float") { - float param; - if (nh.getParam(name, param)) { - param_.attr("set_value")(name, param); + float param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %f", name.c_str(), param); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %f", name.c_str(), param); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); } - } else if (type == "str") { - std::string param; - if (nh.getParam(name, param)) { - param_.attr("set_value")(name, param); - } - } else if (type == "bool") { - bool param; - if (nh.getParam(name, param)) { - param_.attr("set_value")(name, param); - } - } else if (type == "int") { - int param; - if (nh.getParam(name, param)) { - param_.attr("set_value")(name, param); + } else if (type == "str") { + std::string param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %s", name.c_str(), param.c_str()); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %s", name.c_str(), param.c_str()); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } + } else if (type == "bool") { + bool param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %s", name.c_str(), param ? "true" : "false"); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %s", name.c_str(), param ? "true" : "false"); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } + } else if (type == "int") { + int param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %d", name.c_str(), param); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %d", name.c_str(), param); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } } - } + } + + py::dict sub_dict; + // rclcpp::Parameter subscribers; + std::vector parameter_prefixes; + auto parameters = node_->list_parameters(parameter_prefixes, 2); // List all parameters with a maximum depth of 10 - XmlRpc::XmlRpcValue subscribers; - nh.getParam("subscribers", subscribers); - py::dict sub_dict; - for (auto& subscriber : subscribers) { - const char* const name = subscriber.first.c_str(); - const auto& subscriber_params = subscriber.second; - if (!sub_dict.contains(name)) { - sub_dict[name] = py::dict(); - } - for (auto iterat : subscriber_params) { - const char* const key = iterat.first.c_str(); - const auto val = iterat.second; - std::vector arr; - switch (val.getType()) { - case XmlRpc::XmlRpcValue::TypeString: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeInt: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeDouble: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeBoolean: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeArray: - for (int32_t i = 0; i < val.size(); ++i) { - auto elem = static_cast(val[i]); - arr.push_back(elem); + std::map subscriber_params; + if (!node_->get_parameters("subscribers", subscriber_params)) { + RCLCPP_FATAL(node_->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); + rclcpp::shutdown(); + } + auto unique_sub_names = extract_unique_names(subscriber_params); + for (const auto& name : unique_sub_names) { + const char* const name_c = name.c_str(); + if (!sub_dict.contains(name_c)) { + sub_dict[name_c] = py::dict(); } - sub_dict[name][key] = arr; - arr.clear(); - break; - case XmlRpc::XmlRpcValue::TypeStruct: - break; - default: - sub_dict[name][key] = py::cast(val); - break; - } + std::string topic_name; + if(node_->get_parameter("subscribers." + name + ".topic_name", topic_name)){ + const char* topic_name_cstr = "topic_name"; + sub_dict[name_c][topic_name_cstr] = static_cast(topic_name); + std::string data_type; + if(node_->get_parameter("subscribers." + name + ".data_type", data_type)){ + const char* data_type_cstr = "data_type"; + sub_dict[name_c][data_type_cstr] = static_cast(data_type); + } + std::string info_name; + if(node_->get_parameter("subscribers." + name + ".data_type", info_name)){ + const char* info_name_cstr = "info_name"; + sub_dict[name_c][info_name_cstr] = static_cast(info_name); + } + std::string channel_name; + if(node_->get_parameter("subscribers." + name + ".data_type", channel_name)){ + const char* channel_name_cstr = "channel_name"; + sub_dict[name_c][channel_name_cstr] = static_cast(channel_name); + } } } + + + param_.attr("subscriber_cfg") = sub_dict; + // point cloud channel fusion - if (!nh.hasParam("pointcloud_channel_fusions")) { - ROS_WARN("No pointcloud_channel_fusions parameter found. Using default values."); - } - else { - XmlRpc::XmlRpcValue pointcloud_channel_fusion; - nh.getParam("pointcloud_channel_fusions", pointcloud_channel_fusion); - - py::dict pointcloud_channel_fusion_dict; - for (auto& channel_fusion : pointcloud_channel_fusion) { - const char* const name = channel_fusion.first.c_str(); - std::string fusion = static_cast(channel_fusion.second); - if (!pointcloud_channel_fusion_dict.contains(name)) { - pointcloud_channel_fusion_dict[name] = fusion; - } - } - ROS_INFO_STREAM("pointcloud_channel_fusion_dict: " << pointcloud_channel_fusion_dict); - param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict; + std::map pointcloud_channel_fusions_params; + if (node_->get_parameters("pointcloud_channel_fusions", pointcloud_channel_fusions_params)) { + py::dict pointcloud_channel_fusion_dict; + for (const auto& param : pointcloud_channel_fusions_params) { + std::string param_name = param.first; + std::string param_value = param.second.as_string(); + // Extract the string after "pointcloud_channel_fusions." + pointcloud_channel_fusion_dict[param_name.c_str()] = param_value; + } + // Print the dictionary for debugging + for (auto item : pointcloud_channel_fusion_dict) { + RCLCPP_INFO(node_->get_logger(), "pointcloud_channel_fusions Key: %s, Value: %s", std::string(py::str(item.first)).c_str(), std::string(py::str(item.second)).c_str()); + } + } else { + RCLCPP_WARN(node_->get_logger(), "No parameters found for 'pointcloud_channel_fusions'"); } // image channel fusion - if (!nh.hasParam("image_channel_fusions")) { - ROS_WARN("No image_channel_fusions parameter found. Using default values."); - } - else { - XmlRpc::XmlRpcValue image_channel_fusion; - nh.getParam("image_channel_fusions", image_channel_fusion); - - py::dict image_channel_fusion_dict; - for (auto& channel_fusion : image_channel_fusion) { - const char* const name = channel_fusion.first.c_str(); - std::string fusion = static_cast(channel_fusion.second); - if (!image_channel_fusion_dict.contains(name)) { - image_channel_fusion_dict[name] = fusion; - } - } - ROS_INFO_STREAM("image_channel_fusion_dict: " << image_channel_fusion_dict); - param_.attr("image_channel_fusions") = image_channel_fusion_dict; + std::map image_channel_fusions_params; + if (node_->get_parameters("image_channel_fusions", image_channel_fusions_params)) { + py::dict image_channel_fusion_dict; + for (const auto& param : image_channel_fusions_params) { + std::string param_name = param.first; + std::string param_value = param.second.as_string(); + // Extract the string after "pointcloud_channel_fusions." + image_channel_fusion_dict[param_name.c_str()] = param_value; + } + // Print the dictionary for debugging + for (auto item : image_channel_fusion_dict) { + RCLCPP_INFO(node_->get_logger(), "image_channel_fusions Key: %s, Value: %s", std::string(py::str(item.first)).c_str(), std::string(py::str(item.second)).c_str()); + } + } else { + RCLCPP_WARN(node_->get_logger(), "No parameters found for 'image_channel_fusions'"); } + + // Update the cell_n parameters based on the map length and resolution + RCLCPP_INFO(node_->get_logger(), "Updating cell_n parameters based on map length and resolution"); param_.attr("update")(); resolution_ = py::cast(param_.attr("get_value")("resolution")); map_length_ = py::cast(param_.attr("get_value")("true_map_length")); map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); + RCLCPP_INFO(node_->get_logger(), "cell_n: %d", map_n_); + RCLCPP_INFO(node_->get_logger(), "resolution: %f", resolution_); + RCLCPP_INFO(node_->get_logger(), "true_map_length: %f", map_length_); + + enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool(); - nh.param("enable_normal", enable_normal_, false); - nh.param("enable_normal_color", enable_normal_color_, false); } void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, diff --git a/plane_segmentation/cgal5_ament/package.xml b/plane_segmentation/cgal5_ament/package.xml index 672b22b3..d280c209 100644 --- a/plane_segmentation/cgal5_ament/package.xml +++ b/plane_segmentation/cgal5_ament/package.xml @@ -10,7 +10,7 @@ ament_cmake - Boost + boost diff --git a/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt b/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt index 515f0536..47a9f1a4 100644 --- a/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt +++ b/plane_segmentation/grid_map_filters_rsl/CMakeLists.txt @@ -73,9 +73,9 @@ install(DIRECTORY include/${PROJECT_NAME}/ # Clang tools (optional) -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - add_default_clang_tooling() -endif() +# find_package(cmake_clang_tools QUIET) +# if(cmake_clang_tools_FOUND) +# add_default_clang_tooling() +# endif() ament_package() \ No newline at end of file diff --git a/plane_segmentation/grid_map_filters_rsl/package.xml b/plane_segmentation/grid_map_filters_rsl/package.xml index fbea7dc0..786e5637 100644 --- a/plane_segmentation/grid_map_filters_rsl/package.xml +++ b/plane_segmentation/grid_map_filters_rsl/package.xml @@ -14,10 +14,10 @@ ament_cmake - cmake_clang_tools + grid_map_cv grid_map_core - Eigen3 + eigen rosidl_runtime_cpp diff --git a/sensor_processing/semantic_sensor/package.xml b/sensor_processing/semantic_sensor/package.xml index d789afb1..e4b66f31 100644 --- a/sensor_processing/semantic_sensor/package.xml +++ b/sensor_processing/semantic_sensor/package.xml @@ -9,7 +9,7 @@ https://github.com/leggedrobotics/elevation_mapping_semantic_cupy - ament_python + rclpy tf2_ros sensor_msgs @@ -18,6 +18,7 @@ python3 + ament_python