merged ros2->jazzy

.devcontainer/humble/Dockerfile

@@ -8,7 +8,9 @@ ARG USER_GID=1000
 RUN set -ex && \
     groupadd --gid ${USER_GID} ${USERNAME} && \
     useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
-    usermod -a -G sudo ${USERNAME}
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
 
 RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
     chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws

.devcontainer/humble/devcontainer.json

@@ -22,6 +22,18 @@
     },
     "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind",
     "workspaceFolder": "/home/vscode/ros2_ws",
-    "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'"
-    //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"]
+    "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
 }

.devcontainer/jazzy/Dockerfile

@@ -11,7 +11,9 @@ ARG USER_GID=1000
 RUN set -ex && \
     groupadd --gid ${USER_GID} ${USERNAME} && \
     useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
-    usermod -a -G sudo ${USERNAME}
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
 
 RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
     chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws

.devcontainer/jazzy/devcontainer.json

@@ -22,6 +22,18 @@
     },
     "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind",
     "workspaceFolder": "/home/vscode/ros2_ws",
-    "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'"
-    //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"]
+    "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
 }

.devcontainer/kilted/Dockerfile

@@ -11,7 +11,9 @@ ARG USER_GID=1000
 RUN set -ex && \
     groupadd --gid ${USER_GID} ${USERNAME} && \
     useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
-    usermod -a -G sudo ${USERNAME}
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
 
 RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
     chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws

.devcontainer/kilted/devcontainer.json

@@ -22,6 +22,18 @@
     },
     "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind",
     "workspaceFolder": "/home/vscode/ros2_ws",
-    "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'"
-    //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"]
+    "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
 }

.devcontainer/lyrical/Dockerfile

@@ -0,0 +1,22 @@
+
+FROM introlab3it/rtabmap:resolute
+
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
+    chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
+
+RUN echo "source /opt/ros/lyrical/setup.bash" >> /home/${USERNAME}/.bashrc
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc

.devcontainer/lyrical/devcontainer.json

@@ -0,0 +1,39 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile",
+      "pull": true
+    },
+    "remoteUser": "vscode",
+    "customizations": {
+        "vscode": {
+          "extensions": [
+            "ms-vscode.cpptools-themes",
+            "ms-vscode.cmake-tools", 
+            "ms-vscode.cpptools-extension-pack",
+            "ms-azuretools.vscode-docker",
+            "ms-python.python"]
+        }
+      },
+    "settings": {
+      "python.autoComplete.extraPaths": [
+        "/opt/ros/kilted/lib/python3/dist-packages"
+      ],
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind",
+    "workspaceFolder": "/home/vscode/ros2_ws",
+    "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=\"rtabmap nav2_msgs grid_map_ros nav2_costmap_2d nav2_bringup realsense2_camera velodyne\" -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
+}

.devcontainer/noetic/Dockerfile

@@ -8,7 +8,9 @@ ARG USER_GID=1000
 RUN set -ex && \
     groupadd --gid ${USER_GID} ${USERNAME} && \
     useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
-    usermod -a -G sudo ${USERNAME}
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
 
 RUN mkdir -p /home/${USERNAME}/catkin_ws/src && \
     chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/catkin_ws

.devcontainer/noetic/devcontainer.json

@@ -22,6 +22,17 @@
     },
     "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/catkin_ws/src/rtabmap_ros,type=bind",
     "workspaceFolder": "/home/vscode/catkin_ws",
-    "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace"
-    //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"]
+    "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
 }

.devcontainer/rolling/Dockerfile

@@ -1,5 +1,5 @@
 
-FROM ubuntu:24.04
+FROM ubuntu:26.04
 
 ENV DEBIAN_FRONTEND=noninteractive
 

.github/workflows/docker-ros2.yml

@@ -2,8 +2,13 @@ name: docker
 
 on:
   push:
-    branches:
-      - 'ros2'
+    branches: [ ros2 ]
+  pull_request:
+    branches: [ ros2 ]
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
 
 jobs:
   docker:
@@ -12,7 +17,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted-latest]
+        docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted, kilted-latest, lyrical-latest]
         include:
         - docker_tag: humble
           docker_path: 'humble'
@@ -33,8 +38,23 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
+        - docker_tag: kilted
+          docker_path: 'kilted'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
         - docker_tag: kilted-latest
           docker_path: 'kilted/latest'
+          docker_platforms: |
+            linux/amd64
+        #Disabled till rtabmap_ros is released on lyrical
+        #- docker_tag: lyrical
+        #  docker_path: 'lyrical'
+        #  docker_platforms: |
+        #    linux/amd64
+        #    linux/arm64
+        - docker_tag: lyrical-latest
+          docker_path: 'lyrical/latest'
           docker_platforms: |
             linux/amd64
             linux/arm64
@@ -53,6 +73,9 @@ jobs:
         uses: docker/setup-buildx-action@v3
       -
         name: Login to DockerHub
+        # Only needed when pushing; skipped on pull requests (secrets are
+        # unavailable for fork PRs and we don't push there anyway).
+        if: github.event_name != 'pull_request'
         uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
@@ -62,8 +85,8 @@ jobs:
         uses: docker/build-push-action@v6
         with:
           context: .
-          push: true
-          platforms: ${{ matrix.docker_platforms }}
+          push: ${{ github.event_name != 'pull_request' }}
+          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
           file: ./docker/${{ matrix.docker_path }}/Dockerfile
           tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
           no-cache: true

.github/workflows/ros2.yml

@@ -10,6 +10,10 @@ on:
 env:
   BUILD_TYPE: Release
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
 jobs:
   build:
     name: Build ros2 ${{ matrix.ros_distro }}
@@ -20,6 +24,7 @@ jobs:
         include:
         - ros_distro: jazzy
           skip_keys: ''
+          packages: 'rtabmap_ros'
       fail-fast: false
     container:
       image: osrf/ros:${{ matrix.ros_distro }}-desktop-full
@@ -28,12 +33,16 @@ jobs:
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distro }}
+    - run: |
+        DEBIAN_FRONTEND=noninteractive
+        sudo apt update
+        sudo apt upgrade -y
     - run: |
         echo "repositories:\n  introlab/rtabmap:\n    type: git\n    url: https://github.com/introlab/rtabmap.git\n    version: jazzy-devel\n" > /tmp/deps.repos &&\
         cat /tmp/deps.repos
     - uses: ros-tooling/action-ros-ci@v0.4
       with:
-        package-name: rtabmap_ros
+        package-name: ${{ matrix.packages }}
         target-ros2-distro: ${{ matrix.ros_distro }}
         vcs-repo-file-url: /tmp/deps.repos
         rosdep-check: true

docker/humble/latest/Dockerfile

@@ -12,6 +12,6 @@ RUN source /ros_entrypoint.sh && \
     apt-get update && \
     rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

docker/jazzy/latest/Dockerfile

@@ -14,6 +14,6 @@ RUN source /ros_entrypoint.sh && \
     apt-get update && \
     rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

docker/jazzy/superpoint/Dockerfile

@@ -0,0 +1,127 @@
+# Latest version with CUDA 12, to be compatible with Opencv 4.12.0
+FROM nvcr.io/nvidia/pytorch:25.06-py3
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install build dependencies
+RUN apt-get update && apt-get install -y \
+      libsqlite3-dev \
+      git \
+      cmake \
+      libyaml-cpp-dev \
+      software-properties-common \
+      pkg-config \ 
+      wget \
+      curl \
+      build-essential && \
+      apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install ros keys
+RUN export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \
+    curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" && \
+    dpkg -i /tmp/ros2-apt-source.deb
+
+# Install ros dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+        ros-jazzy-ros-base \
+        ros-jazzy-rtabmap-ros \
+        ros-jazzy-ros-environment \
+        ros-jazzy-ament-cmake-auto \
+        ros-jazzy-camera-info-manager \
+        ros-jazzy-librealsense2 \
+        python3-rosdep \
+        python3-flake8-docstrings \
+        python3-pip \
+        python3-pytest-cov \
+        ros-dev-tools && \
+    apt-get remove -y ros-jazzy-rtabmap* libopencv* && \
+    rosdep init && \
+    rosdep update && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Optional: MRPT
+RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y && \
+    apt-get update && apt install libmrpt-poses-dev -y && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Optional: OpenCV with xfeatures2d, cuda and nonfree modules (use same version used by jazzy to avoid cv_bridge conflicts)
+RUN git clone -b 4.12.0 https://github.com/opencv/opencv_contrib.git && \
+    git clone -b 4.12.0 https://github.com/opencv/opencv.git && \
+    cd opencv && \
+    mkdir build && \
+    cd build && \
+    cmake -DOPENCV_EXTRA_MODULES_PATH=/workspace/opencv_contrib/modules \
+          -DCMAKE_CXX_STANDARD=17 \
+          -DCMAKE_CUDA_STANDARD=17 \
+          -DCMAKE_BUILD_TYPE=Release \
+          -DBUILD_SHARED_LIBS=ON \
+          -DBUILD_TESTS=OFF \
+          -DBUILD_PERF_TESTS=OFF \
+          -DOPENCV_ENABLE_NONFREE=ON \
+          -DWITH_VTK=OFF \
+          -DWITH_TBB=ON \
+          -DWITH_CUDA=ON .. && \
+    make -j6 && \
+    make install && \
+    cd /workspace && \
+    rm -rf opencv opencv_contrib
+
+# Optional: OpenGV (multi-camera support)
+RUN git clone https://github.com/laurentkneip/opengv.git && \
+    cd opengv && \
+    git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
+    wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
+    git apply opengv_disable_march_native.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j6 && \
+    make install && \
+    cd /workspace && \
+    rm -r opengv
+
+# Setup catkin workspace
+RUN mkdir -p ros2_ws/src
+COPY . ros2_ws/src/rtabmap_ros
+
+# Get rtabmap library
+# Create Superpoint model with current pytorch version
+# Setup Superglue
+# build ros packages (rebuild all packages depending on opencv)
+RUN source /opt/ros/jazzy/setup.bash && \
+    git clone -b jazzy https://github.com/ros-perception/image_pipeline.git ros2_ws/src/image_pipeline && \
+    git clone -b 4.1.0 https://github.com/ros-perception/vision_opencv.git ros2_ws/src/vision_opencv && \
+    git clone -b jazzy https://github.com/ros-perception/image_transport_plugins.git ros2_ws/src/image_transport_plugins && \
+    git clone -b r/4.56.4 https://github.com/IntelRealSense/realsense-ros.git ros2_ws/src/realsense-ros && \
+    git clone https://github.com/introlab/rtabmap ros2_ws/src/rtabmap && \
+    cd ros2_ws/src/rtabmap/archive/2022-IlluminationInvariant/scripts && \
+    wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth && \
+    wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py && \
+    python3 trace.py && \
+    mv superpoint_v1.pt /workspace/. && \
+    cd /workspace && \
+    git clone https://github.com/magicleap/SuperGluePretrainedNetwork && \
+    cp ros2_ws/src/rtabmap/corelib/src/python/rtabmap_superglue.py SuperGluePretrainedNetwork/. && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j6" && \
+    colcon build --install-base /usr/local/ros --event-handlers console_direct+ --cmake-args \
+        --no-warn-unused-cli \
+        -DTorch_DIR=/usr/local/lib/python3.12/dist-packages/torch/share/cmake/Torch \
+        -DWITH_TORCH=ON \
+        -DWITH_PYTHON=ON \
+        -DRTABMAP_SYNC_MULTI_RGBD=ON \
+        -DCMAKE_BUILD_TYPE=Release \
+        -DBUILD_TESTING=OFF && \
+    cd /workspace && \
+    rm -rf ros2_ws
+
+# Setup ROS entrypoint
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/jazzy/setup.bash"\nsource "/usr/local/ros/setup.bash"\nexec "$@"' > /ros_entrypoint.sh && \
+    chmod +x /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
+RUN source /ros_entrypoint.sh && ldconfig

docker/jazzy/superpoint/README.md

@@ -0,0 +1,59 @@
+Docker image example to include pytorch/CUDA support (SuperPoint, SuperGlue, OpenCV+nonfree+xfeatures2d)
+
+# Create image:
+```bash
+cd rtabmap_ros
+docker build -t rtabmap_ros:superpoint -f docker/jazzy/superpoint/Dockerfile .
+```
+# Example of usage:
+
+We launch the [realsense_d435i_infra.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/realsense_d435i_infra.launch.py) example with arguments to use superpoint + superglue for loop closure detection. Note that visual odometry is done with default parameters in this case.
+
+```bash
+# X11 Setup for rtabmap_viz, not required if you don't launch any UI
+XAUTH=/tmp/.docker.xauth
+touch $XAUTH
+xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
+
+# Docker Run Command
+docker run -it --rm \
+  --user $(id -u) \
+  --privileged \
+  --gpus all \
+  -e LD_PRELOAD="/opt/hpcx/ucc/lib/libucc.so.1" \
+  -e NVIDIA_VISIBLE_DEVICES=all \
+  -e NVIDIA_DRIVER_CAPABILITIES=all \
+  -e DISPLAY=$DISPLAY \
+  -e QT_X11_NO_MITSHM=1 \
+  -e XAUTHORITY=$XAUTH \
+  -v $XAUTH:$XAUTH \
+  -v /tmp/.X11-unix:/tmp/.X11-unix \
+  -e ROS_HOME=/tmp/.ros \
+  --network host \
+  -v ~/.ros:/tmp/.ros \
+  rtabmap_ros:superpoint \
+  ros2 launch rtabmap_examples realsense_d435i_infra.launch.py \
+    args:=" \
+        --SuperPoint/ModelPath /workspace/superpoint_v1.pt \
+        --PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py \
+        --Kp/DetectorStrategy 11 \
+        --Kp/NndrRatio 0.6 \
+        --Vis/CorNNType 6 \
+        --Vis/CorNNDR 0.6 \
+        --Reg/RepeatOnce false \
+        --Vis/CorGuessWinSize 0" \
+     odom_args:=" \
+        --Vis/CorNNType 1 \
+        --Reg/RepeatOnce true \
+        --Vis/CorGuessWinSize 40 \
+        --Vis/CorNNDR 0.8"
+```
+
+The resulting database will be saved to `~/.ros/rtabmap.db` on the host computer. You can also use the `launch.sh` file in this folder for convenience.
+
+To use superpoint for odometry, remove `odom_args` and add this to `args`:
+```bash
+--Vis/FeatureType 11 \
+```
+
+Performance tip: to avoid extracting again in `rtabmap` superpoint features already extracted in `rgbd_odometry`, we would need to edit `realsense_d435i_infra.launch.py` and add the parameter `subscribe_sensor_data:=true` to `rtabmap` and `rtabmap_viz`, then remap `sensor_data:=odom_sensor_data/raw`.

docker/jazzy/superpoint/launch.sh

@@ -0,0 +1,39 @@
+#!/bin/bash
+
+# X11 Setup
+XAUTH=/tmp/.docker.xauth
+touch $XAUTH
+xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
+
+# Docker Run Command
+docker run -it --rm \
+  --user $(id -u) \
+  --privileged \
+  --gpus all \
+  -e LD_PRELOAD="/opt/hpcx/ucc/lib/libucc.so.1" \
+  -e NVIDIA_VISIBLE_DEVICES=all \
+  -e NVIDIA_DRIVER_CAPABILITIES=all \
+  -e DISPLAY=$DISPLAY \
+  -e QT_X11_NO_MITSHM=1 \
+  -e XAUTHORITY=$XAUTH \
+  -v $XAUTH:$XAUTH \
+  -v /tmp/.X11-unix:/tmp/.X11-unix \
+  -e ROS_HOME=/tmp/.ros \
+  --network host \
+  -v ~/.ros:/tmp/.ros \
+  rtabmap_ros:superpoint \
+  ros2 launch rtabmap_examples realsense_d435i_infra.launch.py \
+    args:=" \
+        --SuperPoint/ModelPath /workspace/superpoint_v1.pt \
+        --PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py \
+        --Kp/DetectorStrategy 11 \
+        --Kp/NndrRatio 0.6 \
+        --Vis/CorNNType 6 \
+        --Vis/CorNNDR 0.6 \
+        --Reg/RepeatOnce false \
+        --Vis/CorGuessWinSize 0" \
+     odom_args:=" \
+        --Vis/CorNNType 1 \
+        --Reg/RepeatOnce true \
+        --Vis/CorGuessWinSize 40 \
+        --Vis/CorNNDR 0.8"

docker/kilted/latest/Dockerfile

@@ -14,6 +14,6 @@ RUN source /ros_entrypoint.sh && \
     apt-get update && \
     rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

docker/lyrical/Dockerfile

@@ -0,0 +1,6 @@
+FROM osrf/ros:lyrical-desktop
+# install rtabmap packages
+RUN apt-get update && apt-get install -y \
+    ros-lyrical-rtabmap \
+    ros-lyrical-rtabmap-ros \
+    && rm -rf /var/lib/apt/lists/

docker/lyrical/latest/Dockerfile

@@ -0,0 +1,19 @@
+FROM introlab3it/rtabmap:resolute
+
+RUN source /ros_entrypoint.sh && \
+    mkdir -p ros2_ws/src && \
+    cd ros2_ws/src
+
+COPY . ros2_ws/src/rtabmap_ros
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    rosdep init && \
+    rosdep update && \
+    apt-get update && \
+    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros nav2_costmap_2d" && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    colcon build --packages-skip rtabmap_costmap_plugins rtabmap_ros --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    cd && \
+    rm -rf ros2_ws

rtabmap_conversions/CMakeLists.txt

@@ -28,15 +28,30 @@ find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
-find_package(RTABMap 0.22.0 REQUIRED)
-
-include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
+find_package(RTABMap 0.23.5 REQUIRED)
 
 # libraries
 SET(Libraries
+   image_geometry::image_geometry
+   laser_geometry::laser_geometry
+   pcl_conversions::pcl_conversions
+   std_msgs::std_msgs
+   tf2::tf2
+   tf2_eigen::tf2_eigen
+   tf2_geometry_msgs::tf2_geometry_msgs
+)
+SET(PublicLibraries
+    sensor_msgs::sensor_msgs
+    geometry_msgs::geometry_msgs
+    rtabmap_msgs::rtabmap_msgs
+    rclcpp::rclcpp
+    rtabmap::core
+    cv_bridge::cv_bridge
+    tf2_ros::tf2_ros
+)
+SET(AmentLibraries
    cv_bridge
    geometry_msgs
    image_geometry
@@ -74,7 +89,11 @@ target_include_directories(rtabmap_conversions
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
 )
-ament_target_dependencies(rtabmap_conversions ${Libraries})
+if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+  ament_target_dependencies(rtabmap_conversions ${AmentLibraries})
+ELSE()
+  target_link_libraries(rtabmap_conversions PRIVATE ${Libraries} PUBLIC ${PublicLibraries})
+ENDIF()
 
 IF("$ENV{ROS_DISTRO}" STRLESS "iron")
   target_compile_definitions(rtabmap_conversions PUBLIC -DPRE_ROS_IRON -DPRE_ROS_KILTED)
@@ -85,8 +104,7 @@ ENDIF()
 #############
 ## Install ##
 #############
-
-ament_export_dependencies(${Libraries})
+ament_export_dependencies(${AmentLibraries})
 ament_export_include_directories(include)
 ament_export_targets(${PROJECT_NAME})          # To include downstream with targets
 ament_export_libraries(rtabmap_conversions)    # To include downstream without targets

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MSGCONVERSION_H_
 
 #include "rclcpp/time.hpp"
-#include "tf2_ros/buffer.h"
+#include "tf2_ros/buffer.hpp"
 #include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>

rtabmap_conversions/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_conversions</name>
-  <version>0.22.1</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -28,6 +28,7 @@
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <export>
 	<build_type>ament_cmake</build_type>

rtabmap_conversions/src/MsgConversion.cpp

@@ -191,39 +191,45 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_br
 
 void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
 {
-	if(!image.rgb.data.empty())
+	try
 	{
-		rgb = cv_bridge::toCvShare(image.rgb, trackedObject);
-	}
-	else if(!image.rgb_compressed.data.empty())
-	{
-		rgb = cv_bridge::toCvCopy(image.rgb_compressed);
-	}
-	else
-	{
-		// empty
-		rgb = std::make_shared<cv_bridge::CvImage>();
-	}
-
-	if(!image.depth.data.empty())
-	{
-		depth = cv_bridge::toCvShare(image.depth, trackedObject);
-	}
-	else if(!image.depth_compressed.data.empty())
-	{
-		if(image.depth_compressed.format.compare("jpg")==0)
+		if(!image.rgb.data.empty())
 		{
-			depth = cv_bridge::toCvCopy(image.depth_compressed);
+			rgb = cv_bridge::toCvShare(image.rgb, trackedObject);
+		}
+		else if(!image.rgb_compressed.data.empty())
+		{
+			rgb = cv_bridge::toCvCopy(image.rgb_compressed);
 		}
 		else
 		{
-			cv_bridge::CvImagePtr ptr = std::make_shared<cv_bridge::CvImage>();
-			ptr->header = image.depth_compressed.header;
-			ptr->image = rtabmap::uncompressImage(image.depth_compressed.data);
-			UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
-			ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
-			depth = ptr;
+			// empty
+			rgb = std::make_shared<cv_bridge::CvImage>();
 		}
+
+		if(!image.depth.data.empty())
+		{
+			depth = cv_bridge::toCvShare(image.depth, trackedObject);
+		}
+		else if(!image.depth_compressed.data.empty())
+		{
+			if(image.depth_compressed.format.compare("jpg")==0)
+			{
+				depth = cv_bridge::toCvCopy(image.depth_compressed);
+			}
+			else
+			{
+				cv_bridge::CvImagePtr ptr = std::make_shared<cv_bridge::CvImage>();
+				ptr->header = image.depth_compressed.header;
+				ptr->image = rtabmap::uncompressImage(image.depth_compressed.data);
+				UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
+				ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+				depth = ptr;
+			}
+		}
+	}
+	catch(cv::Exception& e) {
+		UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 	}
 }
 
@@ -348,27 +354,32 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
 			}
 
 			cv::Mat left, right;
-			if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
-			   imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
-			{
-				left = imageRectLeft->image;
+			try {
+				if( imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+					imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
+				{
+					left = imageRectLeft->image;
+				}
+				else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+				{
+					left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
+				}
+				else
+				{
+					left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
+				}
+				if( imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+					imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
+				{
+					right = imageRectRight->image;
+				}
+				else
+				{
+					right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
+				}
 			}
-			else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-			{
-				left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
-			}
-			else
-			{
-				left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
-			}
-			if(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
-			   imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
-			{
-				right = imageRectRight->image;
-			}
-			else
-			{
-				right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
+			catch(cv::Exception& e) {
+				UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 			}
 
 			//
@@ -420,19 +431,24 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
 		}
 
 		cv_bridge::CvImageConstPtr ptrImage = imageMsg;
-		if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
-			imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-			imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
-		{
-			// do nothing
+		try {
+			if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+				imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
+			{
+				// do nothing
+			}
+			else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				ptrImage = cv_bridge::cvtColor(imageMsg, "mono8");
+			}
+			else
+			{
+				ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8");
+			}
 		}
-		else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-		{
-			ptrImage = cv_bridge::cvtColor(imageMsg, "mono8");
-		}
-		else
-		{
-			ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8");
+		catch(cv::Exception& e) {
+			UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 		}
 
 		cv_bridge::CvImageConstPtr ptrDepth = depthMsg;
@@ -835,25 +851,6 @@ rtabmap::CameraModel cameraModelFromROS(
 			D.at<double>(0,4) = camInfo.d[2];
 			D.at<double>(0,5) = camInfo.d[3];
 		}
-		else if(camInfo.d.size()>8)
-		{
-			bool zerosAfter8 = true;
-			for(size_t i=8; i<camInfo.d.size() && zerosAfter8; ++i)
-			{
-				if(camInfo.d[i] != 0.0)
-				{
-					zerosAfter8 = false;
-				}
-			}
-			static bool warned = false;
-			if(!zerosAfter8 && !warned)
-			{
-				UWARN("Camera info conversion: Distortion model is larger than 8, coefficients after 8 are ignored. This message is only shown once.");
-				warned = true;
-			}
-			D = cv::Mat(1, 8, CV_64FC1);
-			memcpy(D.data, camInfo.d.data(), D.cols*sizeof(double));
-		}
 		else
 		{
 			D = cv::Mat(1, camInfo.d.size(), CV_64FC1);
@@ -888,7 +885,12 @@ void cameraModelToROS(
 		sensor_msgs::msg::CameraInfo & camInfo)
 {
 	UASSERT(model.K_raw().empty() || model.K_raw().total() == 9);
-	if(model.K_raw().empty())
+	UASSERT(model.P().empty() || model.P().total() == 12);
+	if(!model.P().empty())
+	{
+		model.P().colRange(0,3).copyTo(cv::Mat(3,3,CV_64FC1, camInfo.k.data()));
+	}
+	else if(model.K_raw().empty())
 	{
 		memset(camInfo.k.data(), 0.0, 9*sizeof(double));
 	}
@@ -897,7 +899,12 @@ void cameraModelToROS(
 		memcpy(camInfo.k.data(), model.K_raw().data, 9*sizeof(double));
 	}
 
-	if(model.D_raw().total() == 6)
+	if(!model.P().empty()) {
+		camInfo.d = std::vector<double>(model.D().cols);
+		memcpy(camInfo.d.data(), model.D().data, model.D().cols*sizeof(double));
+		camInfo.distortion_model = "plumb_bob";
+	}
+	else if(model.D_raw().total() == 6)
 	{
 		camInfo.d = std::vector<double>(4);
 		camInfo.d[0] = model.D_raw().at<double>(0,0);
@@ -921,7 +928,7 @@ void cameraModelToROS(
 	}
 
 	UASSERT(model.R().empty() || model.R().total() == 9);
-	if(model.R().empty())
+	if(model.R().empty() || countNonZero(model.R()) == 0)
 	{
 		cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1);
 		memcpy(camInfo.r.data(), eye.data, 9*sizeof(double));
@@ -931,7 +938,6 @@ void cameraModelToROS(
 		memcpy(camInfo.r.data(), model.R().data, 9*sizeof(double));
 	}
 
-	UASSERT(model.P().empty() || model.P().total() == 12);
 	if(model.P().empty())
 	{
 		memset(camInfo.p.data(), 0.0, 12*sizeof(double));
@@ -969,14 +975,14 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
 		const std::string & frameId,
-		tf2_ros::Buffer & listener,
+		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform)
 {
 	rtabmap::Transform localTransform = getTransform(
 			frameId,
 			leftCamInfo.header.frame_id,
 			leftCamInfo.header.stamp,
-			listener,
+			tfBuffer,
 			waitForTransform);
 	if(localTransform.isNull())
 	{
@@ -987,7 +993,7 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 			leftCamInfo.header.frame_id,
 			rightCamInfo.header.frame_id,
 			leftCamInfo.header.stamp,
-			listener,
+			tfBuffer,
 			waitForTransform);
 	if(stereoTransform.isNull())
 	{
@@ -1146,18 +1152,23 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg)
 		}
 		else
 		{
-			if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
-				leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
-			{
-				left = leftRawPtr->image.clone();
+			try {
+				if( leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+					leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
+				{
+					left = leftRawPtr->image.clone();
+				}
+				else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+				{
+					left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image;
+				}
+				else
+				{
+					left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image;
+				}
 			}
-			else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-			{
-				left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image;
-			}
-			else
-			{
-				left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image;
+			catch(cv::Exception& e) {
+				UFATAL("Fatal error while converting left image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 			}
 		}
 	}
@@ -1179,18 +1190,23 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg)
 		}
 		else
 		{
-			if(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
-				rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-				(!isStereo && 
-				   (rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0||
-					rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
-					rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)))
-			{
-				right = rightRawPtr->image.clone();
+			try{
+				if( rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+					rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+					(!isStereo && 
+					(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0||
+						rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+						rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)))
+				{
+					right = rightRawPtr->image.clone();
+				}
+				else
+				{
+					right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image;
+				}
 			}
-			else
-			{
-				right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image;
+			catch(cv::Exception& e) {
+				UFATAL("Fatal error while converting right image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 			}
 		}
 	}
@@ -1927,7 +1943,7 @@ rtabmap::Landmarks landmarksFromROS(
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const rclcpp::Time & odomStamp,
-		tf2_ros::Buffer & listener,
+		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform,
 		double defaultLinVariance,
 		double defaultAngVariance)
@@ -1945,7 +1961,7 @@ rtabmap::Landmarks landmarksFromROS(
 				frameId,
 				iter->second.first.header.frame_id,
 				iter->second.first.header.stamp,
-				listener,
+				tfBuffer,
 				waitForTransform);
 
 		if(baseToCamera.isNull())
@@ -1965,7 +1981,7 @@ rtabmap::Landmarks landmarksFromROS(
 					odomFrameId,
 					odomStamp,
 					iter->second.first.header.stamp,
-					listener,
+					tfBuffer,
 					waitForTransform);
 			if(!correction.isNull())
 			{
@@ -1994,7 +2010,7 @@ rtabmap::Transform getTransform(
 		const std::string & fromFrameId,
 		const std::string & toFrameId,
 		const rclcpp::Time & stamp,
-		tf2_ros::Buffer &tfBuffer,
+		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform)
 {
 	// TF ready?
@@ -2007,7 +2023,7 @@ rtabmap::Transform getTransform(
 	}
 	catch(tf2::TransformException & ex)
 	{
-		UWARN("(getting transform %s -> %s) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform);
+		UWARN("(getting transform \"%s\" -> \"%s\") %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform);
 	}
 
 	return transform;
@@ -2050,7 +2066,7 @@ bool convertRGBDMsgs(
 		cv::Mat & depth,
 		std::vector<rtabmap::CameraModel> & cameraModels,
 		std::vector<rtabmap::StereoCameraModel> & stereoCameraModels,
-		tf2_ros::Buffer & listener,
+		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform,
 		bool alreadRectifiedImages,
 		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
@@ -2176,7 +2192,7 @@ bool convertRGBDMsgs(
 		}
 
 		// use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb)
-		rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform);
+		rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, tfBuffer, waitForTransform);
 		if(localTransform.isNull())
 		{
 			UERROR("TF of received image for camera %d at time %fs is not set!", i, stamp.seconds());
@@ -2190,7 +2206,7 @@ bool convertRGBDMsgs(
 					odomFrameId,
 					odomStamp,
 					stamp,
-					listener,
+					tfBuffer,
 					waitForTransform);
 			if(sensorT.isNull())
 			{
@@ -2207,19 +2223,24 @@ bool convertRGBDMsgs(
 		if(!imageMsgs.empty())
 		{
 			cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i];
-			if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
-			   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-			   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
-			{
-				// do nothing
+			try {
+				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+				imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
+				{
+					// do nothing
+				}
+				else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+				{
+					ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
+				}
 			}
-			else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-			{
-				ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8");
-			}
-			else
-			{
-				ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
+			catch(cv::Exception& e) {
+				UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 			}
 
 			// initialize
@@ -2270,7 +2291,12 @@ bool convertRGBDMsgs(
 				}
 				else
 				{
-					ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8");
+					try{
+						ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8");
+					}
+					catch(cv::Exception& e) {
+						UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
+					}
 				}
 
 				// initialize
@@ -2329,7 +2355,7 @@ bool convertRGBDMsgs(
 							depthCameraInfoMsgs[i].header.frame_id,
 							cameraInfoMsgs[i].header.frame_id,
 							cameraInfoMsgs[i].header.stamp,
-							listener,
+							tfBuffer,
 							waitForTransform);
 					if(stereoTransform.isNull())
 					{
@@ -2374,7 +2400,7 @@ bool convertRGBDMsgs(
 						cameraInfoMsgs[i].header.frame_id,
 						depthCameraInfoMsgs[i].header.frame_id,
 						cameraInfoMsgs[i].header.stamp,
-						listener,
+						tfBuffer,
 						waitForTransform);
 				}
 				if(stereoTransform.isNull() || stereoTransform.x()<=0)
@@ -2442,7 +2468,7 @@ bool convertStereoMsg(
 		cv::Mat & left,
 		cv::Mat & right,
 		rtabmap::StereoCameraModel & stereoModel,
-		tf2_ros::Buffer & listener,
+		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform,
 		bool alreadyRectified)
 {
@@ -2470,30 +2496,35 @@ bool convertStereoMsg(
 		return false;
 	}
 
-	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
-	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
-	{
-		left = leftImageMsg->image.clone();
+	try{
+		if( leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+			leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
+		{
+			left = leftImageMsg->image.clone();
+		}
+		else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image;
+		}
+		else
+		{
+			left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image;
+		}
+		if( rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+			rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
+		{
+			right = rightImageMsg->image.clone();
+		}
+		else
+		{
+			right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image;
+		}
 	}
-	else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-	{
-		left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image;
-	}
-	else
-	{
-		left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image;
-	}
-	if(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
-	   rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
-	{
-		right = rightImageMsg->image.clone();
-	}
-	else
-	{
-		right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image;
+	catch(cv::Exception& e) {
+		UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
 	}
 
-	rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform);
+	rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tfBuffer, waitForTransform);
 	if(localTransform.isNull())
 	{
 		return false;
@@ -2506,7 +2537,7 @@ bool convertStereoMsg(
 				odomFrameId,
 				odomStamp,
 				leftImageMsg->header.stamp,
-				listener,
+				tfBuffer,
 				waitForTransform);
 		if(sensorT.isNull())
 		{
@@ -2526,7 +2557,7 @@ bool convertStereoMsg(
 				rightCamInfoMsg.header.frame_id,
 				leftCamInfoMsg.header.frame_id,
 				leftCamInfoMsg.header.stamp,
-				listener,
+				tfBuffer,
 				waitForTransform);
 		if(stereoTransform.isNull())
 		{
@@ -2556,7 +2587,7 @@ bool convertStereoMsg(
 				leftCamInfoMsg.header.frame_id,
 				rightCamInfoMsg.header.frame_id,
 				leftCamInfoMsg.header.stamp,
-				listener,
+				tfBuffer,
 				waitForTransform);
 		if(stereoTransform.isNull() || stereoTransform.x()<=0)
 		{
@@ -2753,7 +2784,7 @@ bool convertScan3dMsg(
 		const std::string & odomFrameId,
 		const rclcpp::Time & odomStamp,
 		rtabmap::LaserScan & scan,
-		tf2_ros::Buffer & listener,
+		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform,
 		int maxPoints,
 		float maxRange,
@@ -2762,7 +2793,7 @@ bool convertScan3dMsg(
 	UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height,
 			uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str());
 
-	rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform);
+	rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfBuffer, waitForTransform);
 	if(scanLocalTransform.isNull())
 	{
 		UERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", timestampFromROS(scan3dMsg.header.stamp));
@@ -2777,7 +2808,7 @@ bool convertScan3dMsg(
 				odomFrameId,
 				odomStamp,
 				scan3dMsg.header.stamp,
-				listener,
+				tfBuffer,
 				waitForTransform);
 		if(sensorT.isNull())
 		{

rtabmap_costmap_plugins/CMakeLists.txt

@@ -0,0 +1,99 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_costmap_plugins)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+SET(Libraries
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  nav2_costmap_2d::layers
+  visualization_msgs::visualization_msgs
+)
+SET(AmentLibraries
+  pluginlib
+  rclcpp
+  nav2_costmap_2d
+  visualization_msgs
+)
+
+###########
+## Build ##
+###########
+
+add_library(rtabmap_costmap_plugins SHARED
+       src/voxel_layer.cpp
+)
+target_include_directories(rtabmap_costmap_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+  IF("$ENV{ROS_DISTRO}" STRLESS "jazzy")
+    target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY)
+  ENDIF()
+  ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries})
+ELSE()
+  target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries})
+ENDIF()
+
+
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(rtabmap_costmap_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
+
+# prevent pluginlib from using boost
+target_compile_definitions(rtabmap_costmap_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
+
+add_executable(rtabmap_costmap_voxel_marker src/voxel_marker.cpp)
+IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+  ament_target_dependencies(rtabmap_costmap_voxel_marker ${AmentLibraries})
+ELSE()
+  target_link_libraries(rtabmap_costmap_voxel_marker PRIVATE ${Libraries})
+ENDIF()
+set_target_properties(rtabmap_costmap_voxel_marker PROPERTIES OUTPUT_NAME "voxel_marker")
+
+#############
+## Install ##
+#############
+ament_export_dependencies(${AmentLibraries})
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME})           # To include downstream with targets
+ament_export_libraries(rtabmap_costmap_plugins) # To include downstream without targets
+
+install(TARGETS 
+   rtabmap_costmap_plugins
+   EXPORT ${PROJECT_NAME}
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin
+   INCLUDES DESTINATION include
+)
+
+install(TARGETS 
+   rtabmap_costmap_voxel_marker 
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+   DESTINATION include
+   FILES_MATCHING PATTERN "*.h"
+)
+
+ament_package()

rtabmap_costmap_plugins/costmap_plugins.xml

@@ -0,0 +1,5 @@
+<library path="rtabmap_costmap_plugins">
+  <class type="rtabmap_costmap_plugins::VoxelLayer" base_class_type="nav2_costmap_2d::Layer">
+    <description>Similar to nav2_costmap_2d::VoxelLayer, but can also move along z-axis.</description>
+  </class>
+</library>

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h

@@ -0,0 +1,58 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_
+#define RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((dllexport))
+    #define RTABMAP_COSTMAP_PLUGINS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RTABMAP_COSTMAP_PLUGINS_EXPORT __declspec(dllexport)
+    #define RTABMAP_COSTMAP_PLUGINS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RTABMAP_COSTMAP_PLUGINS_BUILDING_DLL
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_EXPORT
+  #else
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_IMPORT
+  #endif
+  #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE RTABMAP_COSTMAP_PLUGINS_PUBLIC
+  #define RTABMAP_COSTMAP_PLUGINS_LOCAL
+#else
+  #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((visibility("default")))
+  #define RTABMAP_COSTMAP_PLUGINS_IMPORT
+  #if __GNUC__ >= 4
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC __attribute__ ((visibility("default")))
+    #define RTABMAP_COSTMAP_PLUGINS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RTABMAP_COSTMAP_PLUGINS_PUBLIC
+    #define RTABMAP_COSTMAP_PLUGINS_LOCAL
+  #endif
+  #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp

@@ -0,0 +1,293 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+#ifndef RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_
+#define RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_
+
+#include <vector>
+
+#include <rtabmap_costmap_plugins/visibility.h>
+#include <rclcpp/rclcpp.hpp>
+#include <nav2_costmap_2d/layer.hpp>
+#include <nav2_costmap_2d/layered_costmap.hpp>
+#include <nav2_costmap_2d/observation_buffer.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav2_msgs/msg/voxel_grid.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <laser_geometry/laser_geometry.hpp>
+#include <sensor_msgs/msg/point_cloud.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <nav2_costmap_2d/obstacle_layer.hpp>
+#include <nav2_voxel_grid/voxel_grid.hpp>
+
+namespace rtabmap_costmap_plugins
+{
+
+/**
+ * @class VoxelLayer
+ * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment
+ */
+class VoxelLayer : public nav2_costmap_2d::ObstacleLayer
+{
+public:
+  RTABMAP_COSTMAP_PLUGINS_PUBLIC
+  VoxelLayer()
+  : voxel_grid_(0, 0, 0)
+  {
+    costmap_ = NULL;  // this is the unsigned char* member of parent class's parent class Costmap2D
+  }
+
+  /**
+   * @brief Voxel Layer destructor
+   */
+  virtual ~VoxelLayer();
+
+  /**
+   * @brief Initialization process of layer on startup
+   */
+  virtual void onInitialize();
+
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
+  virtual void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x,
+    double * min_y,
+    double * max_x,
+    double * max_y);
+
+  /**
+   * @brief Update the layer's origin to a new pose, often when in a rolling costmap
+   */
+  void updateOrigin(double new_origin_x, double new_origin_y);
+
+  /**
+   * @brief If layer is discretely populated
+   */
+  bool isDiscretized()
+  {
+    return true;
+  }
+
+  /**
+   * @brief Match the size of the master costmap
+   */
+  virtual void matchSize();
+
+  /**
+   * @brief Reset this costmap
+   */
+  virtual void reset();
+
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
+  virtual bool isClearable() {return true;}
+
+protected:
+  /**
+   * @brief Reset internal maps
+   */
+  virtual void resetMaps();
+
+  /**
+   * @brief Use raycasting between 2 points to clear freespace
+   */
+  virtual void raytraceFreespace(
+    const nav2_costmap_2d::Observation & clearing_observation,
+    double * min_x, double * min_y,
+    double * max_x,
+    double * max_y);
+
+  bool publish_voxel_;
+  std::string robot_base_frame_;
+  rclcpp::Publisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
+  nav2_voxel_grid::VoxelGrid voxel_grid_;
+  double z_resolution_, origin_z_;
+  int unknown_threshold_, mark_threshold_, size_z_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
+    clearing_endpoints_pub_;
+
+  /**
+   * @brief Convert world coordinates into map coordinates
+   */
+  inline bool worldToMap3DFloat(
+    double wx, double wy, double wz, double & mx, double & my,
+    double & mz)
+  {
+    if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) {
+      return false;
+    }
+    mx = ((wx - origin_x_) / resolution_);
+    my = ((wy - origin_y_) / resolution_);
+    mz = ((wz - origin_z_) / z_resolution_);
+    if (mx < size_x_ && my < size_y_ && mz < size_z_) {
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Convert world coordinates into map coordinates
+   */
+  inline bool worldToMap3D(
+    double wx, double wy, double wz, unsigned int & mx, unsigned int & my,
+    unsigned int & mz)
+  {
+    if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) {
+      return false;
+    }
+
+    mx = static_cast<unsigned int>((wx - origin_x_) / resolution_);
+    my = static_cast<unsigned int>((wy - origin_y_) / resolution_);
+    mz = static_cast<unsigned int>((wz - origin_z_) / z_resolution_);
+
+    if (mx < size_x_ && my < size_y_ && mz < (unsigned int)size_z_) {
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Convert map coordinates into world coordinates
+   */
+  inline void mapToWorld3D(
+    unsigned int mx, unsigned int my, unsigned int mz, double & wx,
+    double & wy,
+    double & wz)
+  {
+    // returns the center point of the cell
+    wx = origin_x_ + (mx + 0.5) * resolution_;
+    wy = origin_y_ + (my + 0.5) * resolution_;
+    wz = origin_z_ + (mz + 0.5) * z_resolution_;
+  }
+
+  /**
+   * @brief Find L2 norm distance in 3D
+   */
+  inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
+  {
+    return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
+  }
+
+  /**
+   * @brief Get the height of the voxel sizes in meters
+   */
+  double getSizeInMetersZ() const
+  {
+    return (size_z_ - 1 + 0.5) * z_resolution_;
+  }
+
+  /**
+   * @brief  Copy a region of a source map into a destination map
+   * @param  source_map The source map
+   * @param sm_lower_left_x The lower left x point of the source map to start the copy
+   * @param sm_lower_left_y The lower left y point of the source map to start the copy
+   * @param sm_size_x The x size of the source map
+   * @param  dest_map The destination map
+   * @param dm_lower_left_x The lower left x point of the destination map to start the copy
+   * @param dm_lower_left_y The lower left y point of the destination map to start the copy
+   * @param dm_size_x The x size of the destination map
+   * @param region_size_x The x size of the region to copy
+   * @param region_size_y The y size of the region to copy
+   */
+  template<typename data_type>
+  void copyMapRegion3D(
+    data_type * source_map, unsigned int sm_lower_left_x,
+    unsigned int sm_lower_left_y,
+    unsigned int sm_size_x, data_type * dest_map, unsigned int dm_lower_left_x,
+    unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
+    unsigned int region_size_y, int z_shift)
+  {
+    // we'll first need to compute the starting points for each map
+    // this is like getting voxel column. We are not taking into account the z position of the voxel
+    data_type * sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
+    data_type * dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
+
+    uint32_t marked_bits_mask = (data_type) 0xFFFF0000;
+    uint32_t unknown_bits_mask = (data_type) 0x0000FFFF;
+
+    // now, we'll copy the source map into the destination map
+    for (unsigned int i = 0; i < region_size_y; ++i) {
+      memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
+
+      for (unsigned int j = 0; j < region_size_x; j++) {
+        // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
+				if (z_shift > 0) {
+					dm_index[j] =
+							// Shift marked cells, insert zeros for new unknowns
+							((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) |
+							// Shift empty/unknown cells, insert ones for new unknowns
+              (((dm_index[j] & unknown_bits_mask) >> z_shift | (~((data_type) 0) << (sizeof(data_type) * 4 - z_shift))) & unknown_bits_mask);
+
+				} else if (z_shift < 0) {
+					dm_index[j] =
+							// Shift marked cells, insert zeros for new unknowns
+							(dm_index[j] & marked_bits_mask) << z_shift * -1 |
+							// Shift empty/unknown cells, insert ones for new unknowns
+              ((dm_index[j] << z_shift * -1 & unknown_bits_mask) | ~(~((data_type) 0) << z_shift * -1));
+				}
+			}
+
+      sm_index += sm_size_x;
+      dm_index += dm_size_x;
+    }
+  }
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+};
+
+}  // namespace rtabmap_costmap_plugins
+
+#endif  // RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_

rtabmap_costmap_plugins/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtabmap_costmap_plugins</name>
+  <version>0.23.7</version>
+  <description>RTAB-Map's costmap plugins.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <build_depend>ros_environment</build_depend>
+
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>visualization_msgs</depend>
+  
+  <export>
+	<build_type>ament_cmake</build_type>
+  </export>
+
+</package>

rtabmap_costmap_plugins/src/voxel_layer.cpp

@@ -0,0 +1,600 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+#include "rtabmap_costmap_plugins/voxel_layer.hpp"
+
+#include <algorithm>
+#include <cassert>
+#include <vector>
+#include <memory>
+#include <utility>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+#define VOXEL_BITS 16
+PLUGINLIB_EXPORT_CLASS(rtabmap_costmap_plugins::VoxelLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::NO_INFORMATION;
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::FREE_SPACE;
+using rcl_interfaces::msg::ParameterType;
+
+namespace rtabmap_costmap_plugins
+{
+
+void VoxelLayer::onInitialize()
+{
+  nav2_costmap_2d::ObstacleLayer::onInitialize();
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
+  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
+  declareParameter("z_voxels", rclcpp::ParameterValue(10));
+  declareParameter("origin_z", rclcpp::ParameterValue(0.0));
+  declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
+  declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
+  declareParameter("mark_threshold", rclcpp::ParameterValue(0));
+  declareParameter("combination_method", rclcpp::ParameterValue(1));
+  declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
+  declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link"));
+
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
+  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
+  node->get_parameter(name_ + "." + "z_voxels", size_z_);
+  node->get_parameter(name_ + "." + "origin_z", origin_z_);
+  node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
+  node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
+  node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
+  node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
+  node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_);
+
+  int combination_method_param{};
+  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+#ifdef PRE_ROS_JAZZY
+  combination_method_ = combination_method_param;
+#else
+  combination_method_ = combination_method_from_int(combination_method_param);
+#endif
+
+  if (publish_voxel_) {
+    voxel_pub_ = node->create_publisher<nav2_msgs::msg::VoxelGrid>(
+      "voxel_grid", rclcpp::QoS(1).transient_local());
+    //voxel_pub_->on_activate();
+  }
+
+  clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    "clearing_endpoints", rclcpp::QoS(1).transient_local());
+  //clearing_endpoints_pub_->on_activate();
+
+  unknown_threshold_ += (VOXEL_BITS - size_z_);
+  matchSize();
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &VoxelLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
+}
+
+VoxelLayer::~VoxelLayer()
+{
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
+}
+
+void VoxelLayer::matchSize()
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  ObstacleLayer::matchSize();
+  voxel_grid_.resize(size_x_, size_y_, size_z_);
+  assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
+}
+
+void VoxelLayer::reset()
+{
+  // Call the base class method before adding our own functionality
+  ObstacleLayer::reset();
+  resetMaps();
+}
+
+void VoxelLayer::resetMaps()
+{
+  // Call the base class method before adding our own functionality
+  // Note: at the time this was written, ObstacleLayer doesn't implement
+  // resetMaps so this goes to the next layer down Costmap2DLayer which also
+  // doesn't implement this, so it actually goes all the way to Costmap2D
+  ObstacleLayer::resetMaps();
+  voxel_grid_.reset();
+}
+
+void VoxelLayer::updateBounds(
+  double robot_x, double robot_y, double robot_yaw, double * min_x,
+  double * min_y, double * max_x, double * max_y)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  if (rolling_window_) {
+    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+  }
+  if (!enabled_) {
+    return;
+  }
+  useExtraBounds(min_x, min_y, max_x, max_y);
+
+  bool current = true;
+  std::vector<nav2_costmap_2d::Observation> observations, clearing_observations;
+
+  // get the marking observations
+  current = getMarkingObservations(observations) && current;
+
+  // get the clearing observations
+  current = getClearingObservations(clearing_observations) && current;
+
+  // update the global current status
+  current_ = current;
+
+  // raytrace freespace
+  for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
+    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  }
+
+  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  for (std::vector<nav2_costmap_2d::Observation>::const_iterator it = observations.begin(); it != observations.end();
+    ++it)
+  {
+    const nav2_costmap_2d::Observation & obs = *it;
+
+    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+
+    double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
+    double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
+
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
+    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+      // if the obstacle is too low, we won't add it
+      if (*iter_z < min_obstacle_height_) {
+        continue;
+      }
+
+      // if the obstacle is too high or too far away from the robot we won't add it
+      if (*iter_z > max_obstacle_height_) {
+        continue;
+      }
+
+      // compute the squared distance from the hitpoint to the pointcloud's origin
+      double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x) +
+        (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y) +
+        (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
+
+      // if the point is far enough away... we won't consider it
+      if (sq_dist >= sq_obstacle_max_range) {
+        continue;
+      }
+
+      // If the point is too close, do not consider it
+      if (sq_dist < sq_obstacle_min_range) {
+        continue;
+      }
+
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my, mz;
+      if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz)) {
+        continue;
+      }
+
+      // mark the cell in the voxel grid and check if we should also mark it in the costmap
+      if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) {
+        unsigned int index = getIndex(mx, my);
+
+        costmap_[index] = LETHAL_OBSTACLE;
+        touch(
+          static_cast<double>(*iter_x), static_cast<double>(*iter_y),
+          min_x, min_y, max_x, max_y);
+      }
+    }
+  }
+
+  if (publish_voxel_) {
+    auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>();
+    unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
+    grid_msg->size_x = voxel_grid_.sizeX();
+    grid_msg->size_y = voxel_grid_.sizeY();
+    grid_msg->size_z = voxel_grid_.sizeZ();
+    grid_msg->data.resize(size);
+    memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+
+    grid_msg->origin.x = origin_x_;
+    grid_msg->origin.y = origin_y_;
+    grid_msg->origin.z = origin_z_;
+
+    grid_msg->resolutions.x = resolution_;
+    grid_msg->resolutions.y = resolution_;
+    grid_msg->resolutions.z = z_resolution_;
+    grid_msg->header.frame_id = global_frame_;
+    grid_msg->header.stamp = clock_->now();
+
+    voxel_pub_->publish(std::move(grid_msg));
+  }
+
+  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void VoxelLayer::raytraceFreespace(
+  const nav2_costmap_2d::Observation & clearing_observation, double * min_x,
+  double * min_y,
+  double * max_x,
+  double * max_y)
+{
+  auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
+    return;
+  }
+
+  double sensor_x, sensor_y, sensor_z;
+  double ox = clearing_observation.origin_.x;
+  double oy = clearing_observation.origin_.y;
+  double oz = clearing_observation.origin_.z;
+
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) {
+    RCLCPP_WARN(
+      logger_,
+      "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds "
+      "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). "
+      "The costmap cannot raytrace for it.",
+      ox, oy, oz,
+      origin_x_, origin_y_, origin_z_,
+      origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(),
+      origin_z_ + getSizeInMetersZ());
+
+    return;
+  }
+
+  bool publish_clearing_points;
+
+  {
+    auto node = node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+    publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0);
+  }
+
+  clearing_endpoints_->data.clear();
+  clearing_endpoints_->width = clearing_observation.cloud_->width;
+  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->is_dense = true;
+  clearing_endpoints_->is_bigendian = false;
+
+  sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_);
+  modifier.setPointCloud2Fields(
+    3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x(*clearing_endpoints_, "x");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y(*clearing_endpoints_, "y");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z(*clearing_endpoints_, "z");
+
+  // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later
+  double map_end_x = origin_x_ + getSizeInMetersX();
+  double map_end_y = origin_y_ + getSizeInMetersY();
+  double map_end_z = origin_z_ + getSizeInMetersZ();
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    double wpx = *iter_x;
+    double wpy = *iter_y;
+    double wpz = *iter_z;
+
+    double distance = dist(ox, oy, oz, wpx, wpy, wpz);
+    double scaling_fact = 1.0;
+    scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
+    wpx = scaling_fact * (wpx - ox) + ox;
+    wpy = scaling_fact * (wpy - oy) + oy;
+    wpz = scaling_fact * (wpz - oz) + oz;
+
+    double a = wpx - ox;
+    double b = wpy - oy;
+    double c = wpz - oz;
+    double t = 1.0;
+    bool wp_outside = false;
+
+    // we can only raytrace to a maximum z height
+    if (wpz > map_end_z) {
+      // we know we want the vector's z value to be max_z
+      t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
+      wp_outside = true;
+    } else if (wpz < origin_z_) {
+      // and we can only raytrace down to the floor
+      // we know we want the vector's z value to be 0.0
+      t = std::min(t, (origin_z_ - oz) / c);
+      wp_outside = true;
+    }
+
+    // the minimum value to raytrace from is the origin
+    if (wpx < origin_x_) {
+      t = std::min(t, (origin_x_ - ox) / a);
+      wp_outside = true;
+    }
+    if (wpy < origin_y_) {
+      t = std::min(t, (origin_y_ - oy) / b);
+      wp_outside = true;
+    }
+
+    // the maximum value to raytrace to is the end of the map
+    if (wpx > map_end_x) {
+      t = std::min(t, (map_end_x - ox) / a);
+      wp_outside = true;
+    }
+    if (wpy > map_end_y) {
+      t = std::min(t, (map_end_y - oy) / b);
+      wp_outside = true;
+    }
+
+    constexpr double wp_epsilon = 1e-5;
+    if (wp_outside) {
+      if (t > 0.0) {
+        t -= wp_epsilon;
+      } else if (t < 0.0) {
+        t += wp_epsilon;
+      }
+    }
+
+    wpx = ox + a * t;
+    wpy = oy + b * t;
+    wpz = oz + c * t;
+
+    double point_x, point_y, point_z;
+    if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) {
+      unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_);
+      unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_);
+
+
+      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      voxel_grid_.clearVoxelLineInMap(
+        sensor_x, sensor_y, sensor_z, point_x, point_y, point_z,
+        costmap_,
+        unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+        cell_raytrace_max_range, cell_raytrace_min_range);
+
+      updateRaytraceBounds(
+        ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_,
+        clearing_observation.raytrace_min_range_, min_x, min_y,
+        max_x,
+        max_y);
+
+      if (publish_clearing_points) {
+        *clearing_endpoints_iter_x = wpx;
+        *clearing_endpoints_iter_y = wpy;
+        *clearing_endpoints_iter_z = wpz;
+
+        ++clearing_endpoints_iter_x;
+        ++clearing_endpoints_iter_y;
+        ++clearing_endpoints_iter_z;
+      }
+    }
+  }
+
+  if (publish_clearing_points) {
+    clearing_endpoints_->header.frame_id = global_frame_;
+    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
+
+    clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
+  }
+}
+
+void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
+{
+  int cell_oz;
+  // get the global pose of the robot
+  try
+  {
+      geometry_msgs::msg::TransformStamped transformStamped;
+
+      transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, rclcpp::Time(0));
+
+      const double robot_z = transformStamped.transform.translation.z;
+      const double z_grid_height = z_resolution_ * size_z_;
+      const double new_origin_z = robot_z - z_grid_height / 2;
+      cell_oz = int((new_origin_z - origin_z_) / z_resolution_);
+  }
+  catch (tf2::TransformException& ex)
+  {
+      RCLCPP_ERROR(logger_, "%s", ex.what());
+      // If the robot pose is not detected, the origin_z_ will remain the same.
+      cell_oz = 0;
+  }
+
+  // project the new origin into the grid
+  int cell_ox, cell_oy;
+  cell_ox = static_cast<int>((new_origin_x - origin_x_) / resolution_);
+  cell_oy = static_cast<int>((new_origin_y - origin_y_) / resolution_);
+
+  // compute the associated world coordinates for the origin cell
+  // because we want to keep things grid-aligned
+  double new_grid_ox, new_grid_oy, new_grid_oz;
+  new_grid_ox = origin_x_ + cell_ox * resolution_;
+  new_grid_oy = origin_y_ + cell_oy * resolution_;
+  new_grid_oz = origin_z_ + cell_oz * z_resolution_;
+
+  // To save casting from unsigned int to int a bunch of times
+  int size_x = size_x_;
+  int size_y = size_y_;
+
+  // we need to compute the overlap of the new and existing windows
+  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
+  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
+  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
+  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
+  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
+
+  unsigned int cell_size_x = upper_right_x - lower_left_x;
+  unsigned int cell_size_y = upper_right_y - lower_left_y;
+
+  // we need a map to store the obstacles in the window temporarily
+  unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
+  unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  unsigned int * voxel_map = voxel_grid_.getData();
+
+  // copy the local window in the costmap to the local map
+  copyMapRegion(
+    costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x,
+    cell_size_x,
+    cell_size_y);
+  copyMapRegion(
+    voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x,
+    cell_size_x,
+    cell_size_y);
+
+  // we'll reset our maps to unknown space if appropriate
+  resetMaps();
+
+  // update the origin with the appropriate world coordinates
+  origin_x_ = new_grid_ox;
+  origin_y_ = new_grid_oy;
+  origin_z_ = new_grid_oz;
+
+  // compute the starting cell location for copying data back in
+  int start_x = lower_left_x - cell_ox;
+  int start_y = lower_left_y - cell_oy;
+
+  // now we want to copy the overlapping information back into the map, but in its new location
+  copyMapRegion(
+    local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x,
+    cell_size_y);
+  copyMapRegion3D(
+    local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_,
+    cell_size_x,
+    cell_size_y,
+    cell_oz);
+
+  // make sure to clean up
+  delete[] local_map;
+  delete[] local_voxel_map;
+}
+
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+VoxelLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+  bool resize_map_needed = false;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+    if (param_name.find(name_ + ".") != 0) {
+      continue;
+    }
+
+    if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "min_obstacle_height") {
+        min_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "max_obstacle_height") {
+        max_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "origin_z") {
+        origin_z_ = parameter.as_double();
+        resize_map_needed = true;
+      } else if (param_name == name_ + "." + "z_resolution") {
+        z_resolution_ = parameter.as_double();
+        resize_map_needed = true;
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+        current_ = false;
+      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "publish_voxel_map") {
+        RCLCPP_WARN(
+          logger_, "publish voxel map is not a dynamic parameter "
+          "cannot be changed while running. Rejecting parameter update.");
+        continue;
+      }
+
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "z_voxels") {
+        size_z_ = parameter.as_int();
+        resize_map_needed = true;
+      } else if (param_name == name_ + "." + "unknown_threshold") {
+        unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_);
+      } else if (param_name == name_ + "." + "mark_threshold") {
+        mark_threshold_ = parameter.as_int();
+      } else if (param_name == name_ + "." + "combination_method") {
+#ifdef PRE_ROS_JAZZY
+        combination_method_ = parameter.as_int();
+#else
+        combination_method_ = combination_method_from_int(parameter.as_int());
+#endif
+      }
+    }
+  }
+
+  if (resize_map_needed) {
+    matchSize();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace rtabmap_costmap_plugins

rtabmap_costmap_plugins/src/voxel_marker.cpp

@@ -0,0 +1,152 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+
+/**
+ * Modified matlabbe:
+ * Added option to choose between unknown, free and marked cells
+ */
+
+#include <rclcpp/rclcpp.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include <nav2_voxel_grid/voxel_grid.hpp>
+#include <nav2_msgs/msg/voxel_grid.hpp>
+
+namespace rtabmap_costmap_plugins 
+{
+
+// FREE, UNKNOWN, MARKED
+double g_voxel_colors_r[] = {0.0, 1.0, 1.0};
+double g_voxel_colors_g[] = {1.0, 1.0, 0.0};
+double g_voxel_colors_b[] = {1.0, 1.0, 0.0};
+double g_voxel_colors_a[] = {0.5, 0.1, 0.5};
+
+class VoxelMarker: public rclcpp::Node
+{
+public:
+	explicit VoxelMarker(const rclcpp::NodeOptions & options) :
+    rclcpp::Node("voxel_marker", options)
+  {
+    cell_type_ = this->declare_parameter("cell_type", (int)nav2_voxel_grid::VoxelStatus::MARKED);
+    color_r_ = this->declare_parameter("r", g_voxel_colors_r[cell_type_]);
+    color_g_ = this->declare_parameter("g", g_voxel_colors_g[cell_type_]);
+    color_b_ = this->declare_parameter("b", g_voxel_colors_b[cell_type_]);
+    color_a_ = this->declare_parameter("a", g_voxel_colors_a[cell_type_]);  
+    
+    voxel_sub_ = this->create_subscription<nav2_msgs::msg::VoxelGrid>("voxel_grid", rclcpp::QoS(1), std::bind(&VoxelMarker::voxelCallback, this, std::placeholders::_1));
+    marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("visualization_marker", rclcpp::QoS(1));
+
+  }
+	virtual ~VoxelMarker() {}
+
+  void voxelCallback(const nav2_msgs::msg::VoxelGrid::SharedPtr grid)
+  {
+    if (grid->data.empty())
+    {
+      RCLCPP_ERROR(get_logger(), "Received empty voxel grid");
+      return;
+    }
+
+    visualization_msgs::msg::Marker m;
+    m.header.frame_id = grid->header.frame_id;
+    m.header.stamp = grid->header.stamp;
+    m.ns = "voxel_grid";
+    m.id = 0;
+    m.type = visualization_msgs::msg::Marker::CUBE_LIST;
+    m.action = visualization_msgs::msg::Marker::ADD;
+    m.pose.orientation.w = 1.0;
+    m.color.r = color_r_;
+    m.color.g = color_g_;
+    m.color.b = color_b_;
+    m.color.a = color_a_;
+
+    const uint32_t* data = &grid->data.front();
+    const double x_origin = grid->origin.x;
+    const double y_origin = grid->origin.y;
+    const double z_origin = grid->origin.z;
+    const double x_res = grid->resolutions.x;
+    const double y_res = grid->resolutions.y;
+    const double z_res = grid->resolutions.z;
+    const uint32_t x_size = grid->size_x;
+    const uint32_t y_size = grid->size_y;
+    const uint32_t z_size = grid->size_z;
+    for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid)
+    {
+      for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid)
+      {
+        for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid)
+        {
+          nav2_voxel_grid::VoxelStatus status = nav2_voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size,
+                                                                          data);
+
+          if (status == (nav2_voxel_grid::VoxelStatus)cell_type_)
+          {
+            geometry_msgs::msg::Point p;
+            p.x = x_origin + (x_grid + 0.5) * x_res;
+            p.y = y_origin + (y_grid + 0.5) * y_res;
+            p.z = z_origin + (z_grid + 0.5) * z_res;
+            m.points.push_back(p);
+          }
+        }
+      }
+    }
+    m.scale.x = x_res;
+    m.scale.y = y_res;
+    m.scale.z = z_res;
+
+    marker_pub_->publish(m);
+  }
+
+private:
+  int cell_type_;
+  double color_r_;
+  double color_g_;
+  double color_b_;
+  double color_a_;
+
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
+	rclcpp::Subscription<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_sub_;
+};
+
+} // rtabmap_costmap_plugins
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_costmap_plugins::VoxelMarker>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+}

rtabmap_demos/README.md

@@ -8,6 +8,7 @@
 + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam)
 + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam)
 + [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam)
++ [Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF)](#turtlebot3-nav2-2d-lidar-slam-with-fusioncore-imu--wheel-ukf)
 + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam)
 + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam)
 + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam)
@@ -59,6 +60,12 @@
  * Yellow: The map.
 
 ![Peek 2025-07-04 16-57](https://github.com/user-attachments/assets/8869cf57-35a1-4236-bdab-151b88ae2ea1)
+### Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF)
+[turtlebot3_sim_fusioncore_icp_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py) (Jazzy + Gazebo Harmonic)
+
+FusionCore (wheel + IMU UKF) and `icp_odometry` run in a feedback loop: FusionCore's stable `odom` frame seeds scan matching via `guess_frame_id`, and the ICP result feeds back into FusionCore as a second velocity source. See [README](launch/turtlebot3/fusioncore/README.md) for architecture details.
+
+![FusionCore icp_odometry demo](https://github.com/user-attachments/assets/e1e07cfb-74e0-48b9-9bfd-32b68ee5a6ef)
 ### Champ Quadruped Nav2, Elevation Map and VSLAM
 [champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py)
 

rtabmap_demos/launch/husky/husky_slam2d.launch.py

@@ -123,6 +123,7 @@ def generate_launch_description():
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             namespace=robot_ns,
-            parameters=[rtabmap_parameters, shared_parameters],
+            parameters=[rtabmap_parameters, shared_parameters,
+                        {"odometry_node_name": "icp_odometry"}],
             remappings=remappings),
     ])

rtabmap_demos/launch/husky/husky_slam3d.launch.py

@@ -141,6 +141,7 @@ def generate_launch_description():
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             namespace=robot_ns,
-            parameters=[rtabmap_parameters, shared_parameters],
+            parameters=[rtabmap_parameters, shared_parameters,
+                        {"odometry_node_name": "icp_odometry"}],
             remappings=remappings),
     ])

rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py

@@ -129,6 +129,7 @@ def generate_launch_description():
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             namespace=robot_ns,
-            parameters=[rtabmap_parameters, shared_parameters],
+            parameters=[rtabmap_parameters, shared_parameters,
+                        {"odometry_node_name": "icp_odometry"}],
             remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]),
     ])

rtabmap_demos/launch/isaac/isaac_vslam.launch.py

@@ -103,7 +103,8 @@ def launch_setup(context, *args, **kwargs):
             condition=IfCondition(rtabmap_viz),
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             namespace='rtabmap',
-            parameters=[parameters],
+            parameters=[parameters,
+                        {"odometry_node_name": vo_node_prefix+'_odometry'}],
             remappings=remappings),
     ]
 

rtabmap_demos/launch/multisession_mapping_demo.launch.py

@@ -60,6 +60,7 @@ def generate_launch_description():
           'Kp/DetectorStrategy':       '1',       # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries
           'Vis/FeatureType':           '1',       # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries
           'Kp/MaxFeatures':            '400',
+          'Kp/BadSignRatio':           '0.25',    # Kp/BadSignRatio behaves differently than before if Kp/MaxFeatures is not 0, that is now a ratio of Kp/MaxFeatures directly.
           'Reg/Force3DoF':             'true',
           'RGBD/OptimizeMaxError':     '10',
           'Optimizer/Strategy':        '2',       # Referred paper used TORO (0), latest version recommends GTSAM (2)

rtabmap_demos/launch/stereo_outdoor_demo.launch.py

@@ -141,7 +141,8 @@ def generate_launch_description():
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
-            parameters=[parameters],
+            parameters=[parameters,
+                        {"odometry_node_name": 'stereo_odometry'}],
             remappings=remappings),
         Node(
             package='rviz2', executable='rviz2', name="rviz2", output='screen',

rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py

@@ -0,0 +1,201 @@
+# Requirements:
+#   Download one or both rosbags:
+#    * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link
+#    * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link
+#
+# This is the "composition" variant of stereo_outdoor_demo.launch.py: the whole
+# pipeline (image_proc rectification, stereo synchronization, visual odometry
+# and SLAM) runs as composable nodes in a single component container
+# (rtabmap_container). We can set 'use_intra_process_comms' on all of them.
+# That way images are passed between rectify -> disparity/sync -> odometry ->
+# SLAM by pointer, without inter-process serialization/copies.
+#
+# Example:
+#
+#   SLAM:
+#     $ ros2 launch rtabmap_demos stereo_outdoor_demo_composition.launch.py  rviz:=true rtabmap_viz:=true
+#
+#   Rosbag:
+#     $ ros2 bag play stereo_outdoorA.db3 --clock
+#     when done, you can play the secon bag:
+#     $ ros2 bag play stereo_outdoorB.db3 --clock
+#
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node, SetParameter, ComposableNodeContainer, LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    localization = LaunchConfiguration('localization')
+
+    parameters={
+          'frame_id':'base_footprint',
+          'subscribe_rgbd':True,
+          'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs
+          'map_negative_poses_ignored':True,
+          'subscribe_odom_info': True,
+          # RTAB-Map's internal parameters should be strings
+          'OdomF2M/MaxSize': '1000',
+          'GFTT/MinDistance': '10',
+          'GFTT/QualityLevel': '0.00001',
+          #'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d
+          #'Vis/FeatureType': '6'      # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d
+    }
+
+    remappings=[
+         ('rgbd_image', '/stereo_camera/rgbd_image'),
+         ('odom',       '/vo')]
+
+    # Enable zero-copy intra-process communication between all composable nodes
+    # loaded in the container.
+    intra_process = [{'use_intra_process_comms': True}]
+
+    config_rviz = os.path.join(
+        get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
+    )
+
+    # ---- image_proc rectification per camera ----
+    def image_proc_nodes(side, color=False):
+        ns = 'stereo_camera/' + side
+        rectify = ComposableNode(
+            package='image_proc', plugin='image_proc::RectifyNode',
+            name='rectify_color_node' if color else 'rectify_mono_node', namespace=ns,
+            remappings=[
+                ('image',       'image_color' if color else 'image_mono'),
+                ('camera_info', 'camera_info_throttle'),
+                ('image_rect',  'image_rect_color' if color else 'image_rect')],
+            extra_arguments=intra_process)
+        return [
+            ComposableNode(
+                package='image_proc', plugin='image_proc::DebayerNode',
+                name='debayer_node', namespace=ns,
+                extra_arguments=intra_process),
+            rectify,
+        ]
+
+    # ---- rtabmap pipeline (always-on nodes) ----
+    rtabmap_nodes = [
+        # Synchronize stereo data together in a single topic
+        # Issue: stereo_img_proc doesn't produce color and
+        #        grayscale images exactly the same (there is a small
+        #        vertical shift with color), we should use grayscale for
+        #        left and right images to get similar results than on ros1 noetic.
+        ComposableNode(
+            package='rtabmap_sync', plugin='rtabmap_sync::StereoSync',
+            namespace='stereo_camera',
+            remappings=[
+                ('left/image_rect',   'left/image_rect'),
+                ('right/image_rect',  'right/image_rect'),
+                ('left/camera_info',  'left/camera_info_throttle'),
+                ('right/camera_info', 'right/camera_info_throttle')],
+            extra_arguments=intra_process),
+
+        # Visual odometry
+        ComposableNode(
+            package='rtabmap_odom', plugin='rtabmap_odom::StereoOdometry',
+            parameters=[parameters],
+            remappings=remappings,
+            extra_arguments=intra_process),
+    ]
+    
+    # Name of the shared component container.
+    container_name = '/rtabmap_container'
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument('rtabmap_viz',  default_value='false',  description='Launch RTAB-Map UI (optional).'),
+        DeclareLaunchArgument('rviz',         default_value='true',   description='Launch RVIZ (optional).'),
+        DeclareLaunchArgument('localization', default_value='false',  description='Launch in localization mode.'),
+        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz,  description='Configuration path of rviz2.'),
+
+        SetParameter(name='use_sim_time', value=True),
+
+        # Nodes to launch
+
+        # Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc.
+        Node(
+            package='image_transport', executable='republish', name='republish_left', output='screen',
+            namespace='stereo_camera',
+            arguments=['compressed', 'raw'],
+            remappings=[('in/compressed', 'left/image_raw_throttle/compressed'),
+                        ('out',           'left/image_raw')]),
+        Node(
+            package='image_transport', executable='republish', name='republish_right', output='screen',
+            namespace='stereo_camera',
+            arguments=['compressed', 'raw'],
+            remappings=[('in/compressed', 'right/image_raw_throttle/compressed'),
+                        ('out',           'right/image_raw')]),
+
+        # Single component container holding the whole pipeline. All nodes set
+        # use_intra_process_comms=True, so images are passed by pointer.
+        ComposableNodeContainer(
+            name='rtabmap_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            output='screen',
+            composable_node_descriptions=
+                image_proc_nodes('left') +
+                image_proc_nodes('right') +
+                rtabmap_nodes),
+
+        # SLAM mode (loaded into the shared container):
+        # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph
+        # topics by default (transient_local QoS), which is incompatible with
+        # intra-process comms ("intraprocess communication allowed only with
+        # volatile durability"). Setting latch=False makes those topics volatile
+        # so the node can join the zero-copy container. Trade-off: viewers that
+        # start after a map is published won't get the retained last message,
+        # but rtabmap republishes the map as it updates.
+        LoadComposableNodes(
+            condition=UnlessCondition(localization),
+            target_container=container_name,
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper',
+                    parameters=[parameters,
+                      {'delete_db_on_start': True, # Equivalent of '-d': delete the previous database (~/.ros/rtabmap.db)
+                       'latch': False}],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+            ]),
+
+        # Localization mode (loaded into the shared container):
+        LoadComposableNodes(
+            condition=IfCondition(localization),
+            target_container=container_name,
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper',
+                    parameters=[parameters,
+                      {'Mem/IncrementalMemory':'False',
+                       'Mem/InitWMWithAllNodes':'True',
+                       'latch': False}],  # volatile QoS, see SLAM-mode note above
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+            ]),
+
+        # Visualization:
+        # Note: rtabmap_viz is launched as a standalone node, not as a component
+        # in the container above. It is a Qt application and its UI must run in
+        # the process main thread, while components are loaded in container
+        # worker threads. So it cannot be composed and does not benefit from
+        # intra-process comms here (the same applies to rviz2).
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
+            parameters=[parameters,
+                        {"odometry_node_name": 'stereo_odometry'}],
+            remappings=remappings),
+        Node(
+            package='rviz2', executable='rviz2', name="rviz2", output='screen',
+            condition=IfCondition(LaunchConfiguration("rviz")),
+            arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+    ])

rtabmap_demos/launch/turtlebot3/fusioncore/README.md

@@ -0,0 +1,91 @@
+# FusionCore + icp_odometry: TurtleBot3 Gazebo Demo
+
+This demo shows a feedback loop between [FusionCore](https://github.com/manankharwar/fusioncore) and rtabmap's `icp_odometry` where each node tightens the other.
+
+## Architecture
+
+```
+/imu  ──────────────────────┐
+/odom (wheel) ──────────────┤──→  FusionCore (UKF)
+/rtabmap/icp_odometry ──────┘     │
+    ↑                             │ publishes: odom → base_footprint TF
+    │                             │            /fusion/odom
+    │         guess_frame_id: odom│
+    └──────  icp_odometry ←───────┘
+                │  (publish_tf: false)
+                │
+                └──→ /rtabmap/icp_odometry ──→ rtabmap SLAM ──→ map → odom TF
+```
+
+**What each node contributes:**
+
+| Node | Input | Provides |
+|---|---|---|
+| FusionCore | wheels + IMU | stable `odom` frame, continuous state at 100 Hz |
+| icp_odometry | `/scan` + FusionCore's `odom` as initial guess | scan-level pose corrections |
+| FusionCore encoder2 | icp_odometry output | tighter velocity corrections from ICP |
+| rtabmap SLAM | icp_odometry output | global map, loop closures |
+
+FusionCore gives `icp_odometry` a stable initial guess via `guess_frame_id: odom`.
+Better initial guesses mean scan matching succeeds more often and with lower error.
+The ICP result feeds back into FusionCore as a second velocity source (`encoder2`),
+tightening the state estimate further. `Odom/ResetCountdown: 1` lets the system
+auto-recover if ICP loses tracking.
+
+## Simulation vs real hardware
+
+In Gazebo, the DiffDrive plugin produces near-perfect wheel velocities with no slip or
+encoder noise, while the simulated MPU9250 injects Gaussian noise. FusionCore fusing
+both means the noisy IMU slightly degrades what is already a perfect odometry source,
+so the `map → odom` correction on each scan update will be slightly larger than in the
+standard wheel-odometry-only demo. On real hardware this completely inverts: wheel
+encoders accumulate slip, terrain variation, and mechanical error that dwarfs IMU noise,
+and fusion pays off measurably. The sim-tuned IMU noise values in `fusioncore_tb3.yaml`
+(`gyro_noise: 0.002`, `accel_noise: 0.02`) reduce unnecessary filter uncertainty in
+simulation; real MPU9250 users should use the hardware spec values noted in that file.
+
+## Quick start
+
+```bash
+export TURTLEBOT3_MODEL=waffle
+
+ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py
+```
+
+Optional arguments:
+
+```bash
+# Localization mode (requires saved map from a previous mapping run)
+ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true
+
+# Different Gazebo world
+ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house
+```
+
+## Prerequisites
+
+```bash
+sudo apt install ros-jazzy-fusioncore-ros ros-jazzy-turtlebot3-gazebo                  ros-jazzy-rtabmap-ros ros-jazzy-nav2-bringup
+export TURTLEBOT3_MODEL=waffle
+```
+
+## Files
+
+| File | Purpose |
+|---|---|
+| `turtlebot3_sim_fusioncore_icp_demo.launch.py` | Complete demo: Gazebo + FusionCore + rtabmap + Nav2 |
+| `turtlebot3_fusioncore_icp.launch.py` | Core only: FusionCore + icp_odometry + rtabmap (no Gazebo) |
+| `../../params/fusioncore_tb3.yaml` | FusionCore config for TB3 Waffle |
+| `../../params/turtlebot3_fusioncore_icp_nav2_params.yaml` | Nav2 config using `/fusion/odom` |
+
+## Topic and TF summary
+
+| Topic / TF | Publisher | Subscribers |
+|---|---|---|
+| `/imu` | Gazebo | FusionCore |
+| `/odom` | Gazebo (wheel) | FusionCore |
+| `/scan` | Gazebo (lidar) | icp_odometry, rtabmap |
+| `/rtabmap/icp_odometry` | icp_odometry | FusionCore (encoder2), rtabmap |
+| `/fusion/odom` | FusionCore | Nav2 |
+| TF `odom → base_footprint` | FusionCore | icp_odometry (guess), Nav2 |
+| TF `map → odom` | rtabmap | Nav2 |

rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py

@@ -0,0 +1,165 @@
+"""
+FusionCore + icp_odometry feedback loop for TurtleBot3.
+
+Architecture (Option A from rtabmap_ros issue #1418):
+
+  FusionCore (wheels + IMU)
+    |-- publishes: odom -> base_footprint TF,  /fusion/odom
+    |-- provides initial pose guess to icp_odometry via guess_frame_id
+
+  icp_odometry (/scan)
+    |-- guess_frame_id: odom   (uses FusionCore's stable odom as scan match seed)
+    |-- publish_tf: false      (FusionCore owns the odom TF)
+    |-- publishes: /rtabmap/icp_odometry
+
+  FusionCore encoder2
+    |-- topic: /rtabmap/icp_odometry
+    |-- ICP corrections fed back as a second velocity source
+
+  rtabmap SLAM
+    |-- subscribes to /rtabmap/icp_odometry for mapping
+    |-- Odom/ResetCountdown: 1 for auto-recovery if ICP loses tracking
+    |-- publishes: map -> odom TF
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, EmitEvent,
+                             OpaqueFunction, RegisterEventHandler, TimerAction)
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import LifecycleNode, Node
+from launch_ros.event_handlers import OnStateTransition
+from launch_ros.events.lifecycle import ChangeState
+from lifecycle_msgs.msg import Transition
+
+
+def launch_setup(context, *args, **kwargs):
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    localization = LaunchConfiguration('localization').perform(context)
+    localization = localization in ('True', 'true')
+
+    pkg_demos = get_package_share_directory('rtabmap_demos')
+    fusioncore_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3.yaml')
+
+    # ── FusionCore lifecycle node ─────────────────────────────────────────────
+    fc = LifecycleNode(
+        package='fusioncore_ros',
+        executable='fusioncore_node',
+        name='fusioncore',
+        namespace='',
+        output='screen',
+        parameters=[fusioncore_config, {'use_sim_time': use_sim_time}],
+        remappings=[
+            ('/imu/data',    '/imu'),    # TB3 Gazebo IMU topic
+            ('/odom/wheels', '/odom'),   # TB3 Gazebo wheel odometry topic
+        ],
+    )
+
+    # Wait 2 s for node to spin up, then configure
+    configure = TimerAction(
+        period=2.0,
+        actions=[EmitEvent(event=ChangeState(
+            lifecycle_node_matcher=lambda a: a is fc,
+            transition_id=Transition.TRANSITION_CONFIGURE,
+        ))],
+    )
+
+    # As soon as configuring -> inactive, activate
+    activate = RegisterEventHandler(OnStateTransition(
+        target_lifecycle_node=fc,
+        start_state='configuring',
+        goal_state='inactive',
+        entities=[EmitEvent(event=ChangeState(
+            lifecycle_node_matcher=lambda a: a is fc,
+            transition_id=Transition.TRANSITION_ACTIVATE,
+        ))],
+    ))
+
+    # ── icp_odometry ──────────────────────────────────────────────────────────
+    icp_parameters = {
+        'frame_id':               'base_footprint',
+        'odom_frame_id':          'odom',
+        'guess_frame_id':         'odom',
+        'publish_tf':             False,
+        'publish_null_when_lost': False,
+        'use_sim_time':           use_sim_time,
+        'Reg/Strategy':           '1',
+        'Reg/Force3DoF':          'true',
+        'Odom/ResetCountdown':    '1',
+        'RGBD/NeighborLinkRefining': 'True',
+        'Grid/RangeMin':          '0.2',
+    }
+
+    icp_odometry_node = Node(
+        package='rtabmap_odom',
+        executable='icp_odometry',
+        output='screen',
+        parameters=[icp_parameters],
+        remappings=[
+            ('scan', '/scan'),
+            ('odom', '/rtabmap/icp_odometry'),
+        ],
+    )
+
+    # ── rtabmap SLAM ──────────────────────────────────────────────────────────
+    slam_parameters = {
+        'frame_id':              'base_footprint',
+        'odom_frame_id':         'odom',
+        'use_sim_time':          use_sim_time,
+        'subscribe_depth':       False,
+        'subscribe_rgb':         False,
+        'subscribe_scan':        True,
+        'approx_sync':           True,
+        'use_action_for_goal':   True,
+        'Reg/Strategy':          '1',
+        'Reg/Force3DoF':         'true',
+        'RGBD/NeighborLinkRefining': 'True',
+        'Grid/RangeMin':         '0.2',
+        'Optimizer/GravitySigma': '0',
+    }
+
+    if localization:
+        slam_parameters['Mem/IncrementalMemory'] = 'False'
+        slam_parameters['Mem/InitWMWithAllNodes'] = 'True'
+
+    rtabmap_args = [] if localization else ['-d']
+
+    rtabmap_node = Node(
+        package='rtabmap_slam',
+        executable='rtabmap',
+        output='screen',
+        parameters=[slam_parameters],
+        remappings=[
+            ('scan', '/scan'),
+            ('odom', '/rtabmap/icp_odometry'),
+        ],
+        arguments=rtabmap_args,
+    )
+
+    rtabmap_viz_node = Node(
+        package='rtabmap_viz',
+        executable='rtabmap_viz',
+        output='screen',
+        parameters=[slam_parameters, {'odometry_node_name': 'icp_odometry'}],
+        remappings=[
+            ('scan', '/scan'),
+            ('odom', '/rtabmap/icp_odometry'),
+        ],
+    )
+
+    return [fc, configure, activate, icp_odometry_node, rtabmap_node, rtabmap_viz_node]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='true',
+            description='Use simulation (Gazebo) clock if true'),
+
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode (requires existing map)'),
+
+        OpaqueFunction(function=launch_setup),
+    ])

rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py

@@ -0,0 +1,181 @@
+"""
+Complete TurtleBot3 demo: Gazebo (new) + FusionCore + icp_odometry + Nav2.
+
+Launches in order:
+  1. Gazebo Harmonic (via ros_gz_sim) with turtlebot3_world
+  2. Robot state publisher + spawn TurtleBot3
+  3. Custom ros_gz_bridge WITHOUT the odom TF  (FusionCore owns odom->base_footprint)
+  4. FusionCore lifecycle node  (configure -> activate automatically)
+  5. icp_odometry using FusionCore's odom frame as scan-match initial guess
+  6. rtabmap SLAM subscribing to icp_odometry output
+  7. Nav2 using /fusion/odom
+
+Usage:
+  export TURTLEBOT3_MODEL=waffle
+  ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py
+
+  # Localization mode (requires existing map):
+  ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true
+
+  # Different world:
+  ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house
+
+Note on TF ownership:
+  The standard turtlebot3_gazebo bridge forwards the DiffDrive TF to ROS, which
+  conflicts with FusionCore's odom->base_footprint. This demo uses a custom bridge
+  config (fusioncore_tb3_bridge.yaml) that suppresses the Gazebo TF entry.
+  FusionCore is the sole publisher of odom->base_footprint.
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument,
+                             IncludeLaunchDescription, OpaqueFunction)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    if 'TURTLEBOT3_MODEL' not in os.environ:
+        os.environ['TURTLEBOT3_MODEL'] = 'waffle'
+
+    tb3_model   = os.environ['TURTLEBOT3_MODEL']
+    pkg_tb3_gz  = get_package_share_directory('turtlebot3_gazebo')
+    pkg_ros_gz  = get_package_share_directory('ros_gz_sim')
+    pkg_nav2    = get_package_share_directory('nav2_bringup')
+    pkg_demos   = get_package_share_directory('rtabmap_demos')
+
+    world_name = LaunchConfiguration('world').perform(context)
+    world_file = os.path.join(pkg_tb3_gz, 'worlds', f'turtlebot3_{world_name}.world')
+
+    # ── Gazebo server + client ────────────────────────────────────────────────
+    gz_server = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')),
+        launch_arguments={
+            'gz_args': f'-r -s -v2 {world_file}',
+            'on_exit_shutdown': 'true',
+        }.items(),
+    )
+    gz_client = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')),
+        launch_arguments={'gz_args': '-g -v2', 'on_exit_shutdown': 'true'}.items(),
+    )
+
+    # ── Robot state publisher ─────────────────────────────────────────────────
+    robot_state_publisher = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_tb3_gz, 'launch', 'robot_state_publisher.launch.py')),
+        launch_arguments={'use_sim_time': 'true'}.items(),
+    )
+
+    # ── Spawn TurtleBot3 (entity only, no bridge) ─────────────────────────────
+    urdf_path = os.path.join(pkg_tb3_gz, 'models',
+                              f'turtlebot3_{tb3_model}', 'model.sdf')
+    spawn_robot = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-name', tb3_model,
+            '-file', urdf_path,
+            '-x', LaunchConfiguration('x_pose'),
+            '-y', LaunchConfiguration('y_pose'),
+            '-z', '0.01',
+        ],
+        output='screen',
+    )
+
+    # ── Custom bridge: all topics EXCEPT odom TF ──────────────────────────────
+    # FusionCore publishes odom->base_footprint; suppress the Gazebo DiffDrive TF.
+    bridge_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3_bridge.yaml')
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['--ros-args', '-p', f'config_file:={bridge_config}'],
+        output='screen',
+    )
+
+    # Camera image bridge (waffle only)
+    image_bridge = Node(
+        package='ros_gz_image',
+        executable='image_bridge',
+        arguments=['/camera/image_raw'],
+        output='screen',
+    )
+
+    # ── FusionCore + icp_odometry + rtabmap ───────────────────────────────────
+    fusioncore_icp = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_demos, 'launch', 'turtlebot3', 'fusioncore',
+                         'turtlebot3_fusioncore_icp.launch.py')),
+        launch_arguments=[
+            ('localization', LaunchConfiguration('localization')),
+            ('use_sim_time', 'true'),
+        ],
+    )
+
+    # ── Nav2 ──────────────────────────────────────────────────────────────────
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_nav2, 'launch', 'navigation_launch.py')),
+        launch_arguments=[
+            ('use_sim_time', 'true'),
+            ('params_file', PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params',
+                 'turtlebot3_fusioncore_icp_nav2_params.yaml'])),
+        ],
+    )
+
+    rviz = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_nav2, 'launch', 'rviz_launch.py')),
+    )
+
+    set_gz_resource_path = AppendEnvironmentVariable(
+        'GZ_SIM_RESOURCE_PATH',
+        os.path.join(pkg_tb3_gz, 'models'),
+    )
+
+    nodes = [
+        set_gz_resource_path,
+        gz_server,
+        gz_client,
+        robot_state_publisher,
+        spawn_robot,
+        bridge,
+        fusioncore_icp,
+        nav2,
+        rviz,
+    ]
+    if tb3_model == 'waffle':
+        nodes.insert(6, image_bridge)
+
+    return nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+
+        DeclareLaunchArgument(
+            'world', default_value='world',
+            choices=['world', 'house', 'dqn_stage1', 'dqn_stage2',
+                     'dqn_stage3', 'dqn_stage4'],
+            description='Turtlebot3 Gazebo world.'),
+
+        DeclareLaunchArgument(
+            'x_pose', default_value='-2.0',
+            description='Initial X position in Gazebo.'),
+
+        DeclareLaunchArgument(
+            'y_pose', default_value='-0.5',
+            description='Initial Y position in Gazebo.'),
+
+        OpaqueFunction(function=launch_setup),
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py

@@ -20,11 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration
 from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
+from launch.actions import OpaqueFunction
 
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
 
     use_sim_time = LaunchConfiguration('use_sim_time')
     localization = LaunchConfiguration('localization')
+    max_ground_height = LaunchConfiguration('max_ground_height').perform(context)
 
     parameters={
           'frame_id':'base_footprint',
@@ -36,7 +38,7 @@ def generate_launch_description():
           'Grid/3D':'false', # Use 2D occupancy
           'Grid/RangeMax':'3',
           'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
-          'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+          'Grid/MaxGroundHeight': str(max_ground_height), # All points above 5 cm are obstacles
           'Grid/MaxObstacleHeight':'0.4',  # All points over 1 meter are ignored
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
     }
@@ -46,17 +48,7 @@ def generate_launch_description():
           ('rgb/camera_info', '/camera/camera_info'),
           ('depth/image', '/camera/depth/image_raw')]
 
-    return LaunchDescription([
-
-        # Launch arguments
-        DeclareLaunchArgument(
-            'use_sim_time', default_value='true',
-            description='Use simulation (Gazebo) clock if true'),
-        
-        DeclareLaunchArgument(
-            'localization', default_value='false',
-            description='Launch in localization mode.'),
-
+    return [
         # Nodes to launch
         
         # SLAM mode:
@@ -98,4 +90,23 @@ def generate_launch_description():
             remappings=[('cloud', '/camera/cloud'),
                         ('obstacles', '/camera/obstacles'),
                         ('ground', '/camera/ground')]),
-    ])
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='true',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'max_ground_height', default_value='0.05',
+            description='Maximum ground height, everything above is obstacle'),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py

@@ -20,12 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration
 from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
+from launch.actions import OpaqueFunction
 
-
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
 
     use_sim_time = LaunchConfiguration('use_sim_time')
     localization = LaunchConfiguration('localization')
+    max_ground_height = LaunchConfiguration('max_ground_height').perform(context)
 
     parameters={
           'frame_id':'base_footprint',
@@ -42,7 +43,7 @@ def generate_launch_description():
           'Grid/RangeMax':'3',
           'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
           'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map
-          'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+          'Grid/MaxGroundHeight': str(max_ground_height), # All points above are obstacles
           'Grid/MaxObstacleHeight':'0.4',  # All points over 1 meter are ignored
           'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
@@ -53,17 +54,7 @@ def generate_launch_description():
           ('rgb/camera_info', '/camera/camera_info'),
           ('depth/image', '/camera/depth/image_raw')]
 
-    return LaunchDescription([
-
-        # Launch arguments
-        DeclareLaunchArgument(
-            'use_sim_time', default_value='false',
-            description='Use simulation (Gazebo) clock if true'),
-
-        DeclareLaunchArgument(
-            'localization', default_value='false',
-            description='Launch in localization mode.'),
-
+    return [
         # Nodes to launch
         Node(
             package='rtabmap_sync', executable='rgbd_sync', output='screen',
@@ -109,4 +100,23 @@ def generate_launch_description():
             remappings=[('cloud', '/camera/cloud'),
                         ('obstacles', '/camera/obstacles'),
                         ('ground', '/camera/ground')]),
-    ])
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='true',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'max_ground_height', default_value='0.05',
+            description='Maximum ground height, everything above is obstacle'),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py

@@ -76,7 +76,8 @@ def launch_setup(context, *args, **kwargs):
         # Visualization
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[parameters],
+            parameters=[parameters,
+                        {"odometry_node_name": 'icp_odometry'}],
             remappings=remappings),
     ]
 

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py

@@ -13,8 +13,30 @@
 #          </joint> 
 #     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
 #     4) Add <link name="camera_rgb_frame"/>
-#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
-#     6) Change image width/height from 1920x1080 to 640x480
+#     5) Change image width/height from 1920x1080 to 640x480
+#     6) [ROS2 HUMBLE] Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) [ROS2 JAZZY] Change <gz_frame_id>camera_rgb_frame</gz_frame_id> to <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#     7) [ROS2 JAZZY] Add the following just after <sensor name="camera" ...> section
+#        <sensor name="depth" type="depth">
+#            <always_on>true</always_on>
+#            <visualize>true</visualize>
+#            <update_rate>30</update_rate>
+#            <topic>camera/depth/image_raw</topic>
+#            <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#            <camera name="intel_realsense_r200_depth">
+#            <camera_info_topic>camera/depth/camera_info</camera_info_topic>
+#            <horizontal_fov>1.02974</horizontal_fov>
+#            <image>
+#                <width>640</width>
+#                <height>480</height>
+#                <format>R8G8B8</format>
+#            </image>
+#            <clip>
+#                <near>0.02</near>
+#                <far>300</far>
+#            </clip>
+#            </camera>
+#        </sensor>
 # Example:
 #   $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py
 #
@@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
@@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs):
 
     world = LaunchConfiguration('world').perform(context)
     
-    nav2_params_file = PathJoinSubstitution(
-        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
-    )
+    if ROS_DISTRO == 'humble':
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
+    else:
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
 
     # Paths
     gazebo_launch = PathJoinSubstitution(
@@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs):
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py'])
 
     # Includes
-    gazebo = IncludeLaunchDescription(
+    gazebo = [IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
             ('x_pose', LaunchConfiguration('x_pose')),
             ('y_pose', LaunchConfiguration('y_pose'))
         ]
-    )
+    )]
+    if ROS_DISTRO != 'humble':
+        start_gazebo_ros_depth_image_bridge_cmd = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['/camera/depth/image_raw'],
+            output='screen',
+        )
+        gazebo.append(start_gazebo_ros_depth_image_bridge_cmd)
+    
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
         launch_arguments=[
@@ -77,20 +116,25 @@ def launch_setup(context, *args, **kwargs):
     rviz = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rviz_launch])
     )
+    
+    max_ground_height = '0.05'
+    if ROS_DISTRO == 'jazzy':
+        max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate
+
     rtabmap = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rtabmap_launch]),
         launch_arguments=[
             ('localization', LaunchConfiguration('localization')),
-            ('use_sim_time', 'true')
+            ('use_sim_time', 'true'),
+            ('max_ground_height', max_ground_height)
         ]
     )
     return [
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gazebo
-    ]
+        rtabmap
+    ] + gazebo
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py

@@ -13,8 +13,30 @@
 #          </joint> 
 #     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
 #     4) Add <link name="camera_rgb_frame"/>
-#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
-#     6) Change image width/height from 1920x1080 to 640x480
+#     5) Change image width/height from 1920x1080 to 640x480
+#     6) [ROS2 HUMBLE] Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) [ROS2 JAZZY] Change <gz_frame_id>camera_rgb_frame</gz_frame_id> to <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#     7) [ROS2 JAZZY] Add the following just after <sensor name="camera" ...> section (under same link)
+#        <sensor name="depth" type="depth">
+#            <always_on>true</always_on>
+#            <visualize>true</visualize>
+#            <update_rate>30</update_rate>
+#            <topic>camera/depth/image_raw</topic>
+#            <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#            <camera name="intel_realsense_r200_depth">
+#            <camera_info_topic>camera/depth/camera_info</camera_info_topic>
+#            <horizontal_fov>1.02974</horizontal_fov>
+#            <image>
+#                <width>640</width>
+#                <height>480</height>
+#                <format>R8G8B8</format>
+#            </image>
+#            <clip>
+#                <near>0.02</near>
+#                <far>300</far>
+#            </clip>
+#            </camera>
+#        </sensor>
 # Example:
 #   $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py
 #
@@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
@@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs):
 
     world = LaunchConfiguration('world').perform(context)
     
-    nav2_params_file = PathJoinSubstitution(
-        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
-    )
+    if ROS_DISTRO == 'humble':
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
+    else:
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
 
     # Paths
     gazebo_launch = PathJoinSubstitution(
@@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs):
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py'])
 
     # Includes
-    gazebo = IncludeLaunchDescription(
+    gazebo = [IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
             ('x_pose', LaunchConfiguration('x_pose')),
             ('y_pose', LaunchConfiguration('y_pose'))
         ]
-    )
+    )]
+    if ROS_DISTRO != 'humble':
+        start_gazebo_ros_depth_image_bridge_cmd = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['/camera/depth/image_raw'],
+            output='screen',
+        )
+        gazebo.append(start_gazebo_ros_depth_image_bridge_cmd)
+
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
         launch_arguments=[
@@ -77,6 +116,7 @@ def launch_setup(context, *args, **kwargs):
     rviz = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rviz_launch])
     )
+
     rtabmap = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rtabmap_launch]),
         launch_arguments=[
@@ -88,9 +128,8 @@ def launch_setup(context, *args, **kwargs):
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gazebo
-    ]
+        rtabmap
+    ] + gazebo
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py

@@ -13,8 +13,30 @@
 #          </joint> 
 #     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
 #     4) Add <link name="camera_rgb_frame"/>
-#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
-#     6) Change image width/height from 1920x1080 to 640x480
+#     5) Change image width/height from 1920x1080 to 640x480
+#     6) [ROS2 HUMBLE] Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) [ROS2 JAZZY] Change <gz_frame_id>camera_rgb_frame</gz_frame_id> to <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#     7) [ROS2 JAZZY] Add the following just after <sensor name="camera" ...> section (under same link)
+#        <sensor name="depth" type="depth">
+#            <always_on>true</always_on>
+#            <visualize>true</visualize>
+#            <update_rate>30</update_rate>
+#            <topic>camera/depth/image_raw</topic>
+#            <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#            <camera name="intel_realsense_r200_depth">
+#            <camera_info_topic>camera/depth/camera_info</camera_info_topic>
+#            <horizontal_fov>1.02974</horizontal_fov>
+#            <image>
+#                <width>640</width>
+#                <height>480</height>
+#                <format>R8G8B8</format>
+#            </image>
+#            <clip>
+#                <near>0.02</near>
+#                <far>300</far>
+#            </clip>
+#            </camera>
+#        </sensor>
 #     7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans 
 #        hitting the robot itself
 # Example:
@@ -30,9 +52,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
@@ -47,9 +72,14 @@ def launch_setup(context, *args, **kwargs):
 
     world = LaunchConfiguration('world').perform(context)
     
-    nav2_params_file = PathJoinSubstitution(
-        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml']
-    )
+    if ROS_DISTRO == 'humble':
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_scan_nav2_params.yaml']
+        )
+    else:
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml']
+        )
 
     # Paths
     gazebo_launch = PathJoinSubstitution(
@@ -62,13 +92,22 @@ def launch_setup(context, *args, **kwargs):
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py'])
 
     # Includes
-    gazebo = IncludeLaunchDescription(
+    gazebo = [IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
             ('x_pose', LaunchConfiguration('x_pose')),
             ('y_pose', LaunchConfiguration('y_pose'))
         ]
-    )
+    )]
+    if ROS_DISTRO != 'humble':
+        start_gazebo_ros_depth_image_bridge_cmd = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['/camera/depth/image_raw'],
+            output='screen',
+        )
+        gazebo.append(start_gazebo_ros_depth_image_bridge_cmd)
+
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
         launch_arguments=[
@@ -79,20 +118,25 @@ def launch_setup(context, *args, **kwargs):
     rviz = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rviz_launch])
     )
+    
+    max_ground_height = '0.05'
+    if ROS_DISTRO == 'jazzy':
+        max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate
+    
     rtabmap = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rtabmap_launch]),
         launch_arguments=[
             ('localization', LaunchConfiguration('localization')),
-            ('use_sim_time', 'true')
+            ('use_sim_time', 'true'),
+            ('max_ground_height', max_ground_height)
         ]
     )
     return [
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gazebo
-    ]
+        rtabmap
+    ] + gazebo
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py

@@ -14,18 +14,22 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
 
     # Directories
+    pkg_turtlebot3_gazebo = get_package_share_directory(
+        'turtlebot3_gazebo')
     pkg_nav2_bringup = get_package_share_directory(
         'nav2_bringup')
     pkg_rtabmap_demos = get_package_share_directory(
@@ -37,14 +41,26 @@ def launch_setup(context, *args, **kwargs):
     icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
     if icp_odometry:
         # modified nav2 params to use icp_odom instead odom frame
-        nav2_params_file = PathJoinSubstitution(
-            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml']
-        )
+        if ROS_DISTRO == 'humble':
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_scan_nav2_params.yaml']
+            )
+        else:
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml']
+            )
     else:
-        # original nav2 params
-        nav2_params_file = PathJoinSubstitution(
-            [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml']
-        )
+        if ROS_DISTRO == 'humble':
+            # original nav2 params
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml']
+            )
+        else:
+            # original nav2 params but with "enable_stamped_cmd_vel: True"
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_nav2_params.yaml']
+            )
+            
 
     # Paths
     nav2_launch = PathJoinSubstitution(
@@ -53,56 +69,6 @@ def launch_setup(context, *args, **kwargs):
         [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
     rtabmap_launch = PathJoinSubstitution(
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py'])
-
-    # To use ICP odometry, we should increase clock rate of gazebo, we copied content of
-    # turtlebot3_gazebo/launch/turtlebot3_world.launch here
-    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
-    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
-
-    world = os.path.join(
-        get_package_share_directory('turtlebot3_gazebo'),
-        'worlds',
-        f'turtlebot3_{world_name}.world'
-    )
-
-    import tempfile
-    with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file:
-        clock_override_file.write("---\n"+
-                  "gazebo:\n"+
-                  "    ros__parameters:\n"+
-                  "        publish_rate: 100.0")
-
-    gzserver_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
-        ),
-        launch_arguments={
-            'world': world,
-            'params_file': clock_override_file.name}.items()
-    )
-
-    gzclient_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
-        )
-    )
-
-    robot_state_publisher_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
-        ),
-        launch_arguments={'use_sim_time': 'true'}.items()
-    )
-
-    spawn_turtlebot_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
-        ),
-        launch_arguments={
-            'x_pose': LaunchConfiguration('x_pose'),
-            'y_pose': LaunchConfiguration('y_pose')
-        }.items()
-    )
     
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
@@ -121,16 +87,83 @@ def launch_setup(context, *args, **kwargs):
             ('use_sim_time', 'true')
         ]
     )
+        
+    # To use ICP odometry, we should increase clock rate of gazebo (humble), we copied content of
+    # turtlebot3_gazebo/launch/turtlebot3_world.launch here.
+    turtlebot3_nodes = []
+    if ROS_DISTRO == 'humble':
+        pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+        import tempfile
+        with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file:
+            clock_override_file.write("---\n"+
+                    "gazebo:\n"+
+                    "    ros__parameters:\n"+
+                    "        publish_rate: 100.0")
+
+        world = os.path.join(
+                pkg_turtlebot3_gazebo,
+                'worlds',
+                f'turtlebot3_{world_name}.world'
+        )
+
+        gzserver_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+            ),
+            launch_arguments={
+                'world': world,
+                'params_file': clock_override_file.name}.items()
+        )
+
+        gzclient_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+            )
+        )
+        
+        robot_state_publisher_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_turtlebot3_gazebo, 'launch', 'robot_state_publisher.launch.py')
+            ),
+            launch_arguments={'use_sim_time': 'true'}.items()
+        )
+        spawn_turtlebot_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_turtlebot3_gazebo, 'launch', 'spawn_turtlebot3.launch.py')
+            ),
+            launch_arguments={
+                'x_pose': LaunchConfiguration('x_pose'),
+                'y_pose': LaunchConfiguration('y_pose')
+            }.items()
+        )
+        
+        set_env_vars_resources = AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                os.path.join(pkg_turtlebot3_gazebo, 'models'))
+        turtlebot3_nodes = [
+            gzserver_cmd,
+            gzclient_cmd,
+            robot_state_publisher_cmd,
+            spawn_turtlebot_cmd,
+            set_env_vars_resources
+        ]
+    else:
+        gazebo_launch = PathJoinSubstitution([pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world_name}.launch.py'])
+        gazebo = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([gazebo_launch]),
+            launch_arguments=[
+                ('x_pose', LaunchConfiguration('x_pose')),
+                ('y_pose', LaunchConfiguration('y_pose'))
+            ]
+        )
+        turtlebot3_nodes = [gazebo]
+
     return [
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gzserver_cmd,
-        gzclient_cmd,
-        robot_state_publisher_cmd,
-        spawn_turtlebot_cmd
-    ]
+        rtabmap] + turtlebot3_nodes
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py

@@ -114,6 +114,7 @@ def generate_launch_description():
         Node(
             condition=IfCondition(rtabmap_viz),
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[rtabmap_parameters, shared_parameters],
+            parameters=[rtabmap_parameters, shared_parameters,
+                        {"odometry_node_name": 'icp_odometry'}],
             remappings=remappings),
     ])

rtabmap_demos/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_demos</name>
-  <version>0.22.1</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's demo launch files.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_demos/params/fusioncore_tb3.yaml

@@ -0,0 +1,63 @@
+# FusionCore config for TurtleBot3 Waffle (Gazebo Harmonic)
+#
+# Platform:  TurtleBot3 Waffle (simulated)
+# IMU:       MPU9250 (9-axis, magnetometer unreliable in sim)
+# GPS:       None (indoor / simulation)
+# LiDAR:     HLS-LFCD2 via /scan (used by icp_odometry, not fused here directly)
+# encoder2:  rtabmap icp_odometry output on /rtabmap/icp_odometry
+#
+# Architecture: FusionCore (wheels + IMU) provides the odom frame.
+# icp_odometry uses that odom frame as its initial scan-matching guess
+# (guess_frame_id: odom). The ICP output feeds back into FusionCore
+# as a second velocity source (encoder2). Each tightens the other.
+
+fusioncore:
+  ros__parameters:
+    base_frame: base_footprint
+    odom_frame: odom
+    publish_rate: 100.0
+    publish.force_2d: true
+
+    # Gazebo Harmonic prefixes sensor frames with the model name (waffle/imu_link/tb3_imu).
+    # Override to the TF frame that robot_state_publisher actually publishes.
+    imu.frame_id: "imu_link"
+
+    # MPU9250: magnetometer disabled (unreliable in sim / near motors)
+    imu.has_magnetometer: false
+    # Simulation-tuned noise values for Gazebo Harmonic MPU9250 plugin.
+    # Real MPU9250 hardware: gyro ~0.005 rad/s, accel ~0.1 m/s2.
+    # Gazebo injects lower noise than the real sensor, so tighter values
+    # reduce unnecessary filter uncertainty in sim without affecting real-hardware users
+    # (who should revert to the hardware spec values above).
+    imu.gyro_noise: 0.002             # rad/s  (Gazebo sim-tuned; real MPU9250: 0.005)
+    imu.accel_noise: 0.02             # m/s2   (Gazebo sim-tuned; real MPU9250: 0.1)
+    imu.remove_gravitational_acceleration: false
+
+    # Wheel odometry noise (TB3 differential drive, simulated)
+    encoder.vel_noise: 0.05           # m/s
+    encoder.yaw_noise: 0.02           # rad/s
+
+    # ICP odometry as second velocity source
+    encoder2.topic: "/rtabmap/icp_odometry"
+
+    outlier_rejection: true
+    outlier_threshold_imu: 15.09
+    outlier_threshold_enc: 11.34
+
+    adaptive.imu: true
+    adaptive.encoder: true
+    adaptive.window: 50
+    adaptive.alpha: 0.01
+
+    zupt.enabled: true
+    zupt.velocity_threshold: 0.08     # m/s: slightly loose for ICP jitter
+    zupt.angular_threshold: 0.05      # rad/s
+    zupt.noise_sigma: 0.01
+
+    ukf.q_position: 0.01
+    ukf.q_orientation: 1.0e-9
+    ukf.q_velocity: 0.1
+    ukf.q_angular_vel: 0.1
+    ukf.q_acceleration: 1.0
+    ukf.q_gyro_bias: 1.0e-5
+    ukf.q_accel_bias: 1.0e-5

rtabmap_demos/params/fusioncore_tb3_bridge.yaml

@@ -0,0 +1,47 @@
+# ros_gz_bridge config for the FusionCore + icp_odometry demo.
+#
+# Identical to turtlebot3_waffle_bridge.yaml EXCEPT the 'tf' entry is removed.
+# FusionCore publishes odom -> base_footprint TF directly, so the DiffDrive
+# plugin's TF must not be forwarded to avoid a competing transform.
+
+- ros_topic_name: "clock"
+  gz_topic_name: "clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "joint_states"
+  gz_topic_name: "joint_states"
+  ros_type_name: "sensor_msgs/msg/JointState"
+  gz_type_name: "gz.msgs.Model"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "odom"
+  gz_topic_name: "odom"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "cmd_vel"
+  gz_topic_name: "cmd_vel"
+  ros_type_name: "geometry_msgs/msg/TwistStamped"
+  gz_type_name: "gz.msgs.Twist"
+  direction: ROS_TO_GZ
+
+- ros_topic_name: "imu"
+  gz_topic_name: "imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "scan"
+  gz_topic_name: "scan"
+  ros_type_name: "sensor_msgs/msg/LaserScan"
+  gz_type_name: "gz.msgs.LaserScan"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "camera/camera_info"
+  gz_topic_name: "camera/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS

rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml

@@ -0,0 +1,287 @@
+# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source.
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_assisted_teleop_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_goal_updated_controller_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_assisted_teleop_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: ground obstacles
+        ground:
+          topic: /camera/ground
+          max_obstacle_height: 0.4
+          clearing: True
+          marking: False
+          data_type: "PointCloud2"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+        obstacles:
+          topic: /camera/obstacles
+          max_obstacle_height: 0.4
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    # Overridden in launch by the "map" launch configuration or provided default value.
+    # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
+    yaml_filename: ""
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors/AssistedTeleop"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: True
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.26, 0.0, 1.0]
+    min_velocity: [-0.26, 0.0, -1.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml

@@ -0,0 +1,301 @@
+# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap.
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_assisted_teleop_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_goal_updated_controller_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_assisted_teleop_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan ground obstacles
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+        ground:
+          topic: /camera/ground
+          max_obstacle_height: 0.4
+          clearing: True
+          marking: False
+          data_type: "PointCloud2"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+        obstacles:
+          topic: /camera/obstacles
+          max_obstacle_height: 0.4
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors/AssistedTeleop"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: True
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.26, 0.0, 1.0]
+    min_velocity: [-0.26, 0.0, -1.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml

@@ -0,0 +1,295 @@
+# Modified to use icp_odom frame
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_assisted_teleop_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_goal_updated_controller_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_assisted_teleop_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: icp_odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors/AssistedTeleop"
+    global_frame: icp_odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: True
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.26, 0.0, 1.0]
+    min_velocity: [-0.26, 0.0, -1.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml

@@ -0,0 +1,280 @@
+# Nav2 parameters for FusionCore + icp_odometry TurtleBot3 demo.
+#
+# Key difference from stock nav2 params:
+#   - odom_topic: /fusion/odom  (FusionCore output, not /odom or /odometry/filtered)
+#   - No AMCL: rtabmap handles the map -> odom transform via its SLAM output.
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: true
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /fusion/odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ['navigate_to_pose', 'navigate_through_poses']
+    navigate_to_pose:
+      plugin: 'nav2_bt_navigator::NavigateToPoseNavigator'
+    navigate_through_poses:
+      plugin: 'nav2_bt_navigator::NavigateThroughPosesNavigator'
+
+controller_server:
+  ros__parameters:
+    use_sim_time: true
+    enable_stamped_cmd_vel: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    odom_topic: /fusion/odom
+    progress_checker_plugins: ['progress_checker']
+    goal_checker_plugins: ['general_goal_checker']
+    controller_plugins: ['FollowPath']
+    progress_checker:
+      plugin: 'nav2_controller::SimpleProgressChecker'
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    general_goal_checker:
+      stateful: true
+      plugin: 'nav2_controller::SimpleGoalChecker'
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    FollowPath:
+      plugin: 'nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController'
+      desired_linear_vel: 0.2
+      lookahead_dist: 0.6
+      min_lookahead_dist: 0.3
+      max_lookahead_dist: 0.9
+      lookahead_time: 1.5
+      rotate_to_heading_angular_vel: 1.8
+      transform_tolerance: 0.1
+      use_velocity_scaled_lookahead_dist: false
+      min_approach_linear_velocity: 0.05
+      approach_velocity_scaling_dist: 0.6
+      use_collision_detection: true
+      max_allowed_time_to_collision_up_to_goal: 1.0
+      use_regulated_linear_velocity_scaling: true
+      use_fixed_curvature_lookahead: false
+      curvature_feedforward_gain: 1.0
+      use_cost_regulated_linear_velocity_scaling: false
+      regulated_linear_scaling_min_radius: 0.9
+      regulated_linear_scaling_min_speed: 0.25
+      use_rotate_to_heading: true
+      allow_reversing: false
+      rotate_to_heading_min_angle: 0.785
+      max_angular_accel: 3.2
+      max_robot_pose_search_dist: 10.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: true
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ['obstacle_layer', 'inflation_layer']
+      obstacle_layer:
+        plugin: 'nav2_costmap_2d::ObstacleLayer'
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: true
+          marking: true
+          data_type: 'LaserScan'
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: 'nav2_costmap_2d::InflationLayer'
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: true
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: true
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ['static_layer', 'obstacle_layer', 'inflation_layer']
+      static_layer:
+        plugin: 'nav2_costmap_2d::StaticLayer'
+        map_subscribe_transient_local: true
+      obstacle_layer:
+        plugin: 'nav2_costmap_2d::ObstacleLayer'
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: true
+          marking: true
+          data_type: 'LaserScan'
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: 'nav2_costmap_2d::InflationLayer'
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: true
+
+planner_server:
+  ros__parameters:
+    use_sim_time: true
+    expected_planner_frequency: 20.0
+    planner_plugins: ['GridBased']
+    GridBased:
+      plugin: 'nav2_navfn_planner::NavfnPlanner'
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: true
+    enable_stamped_cmd_vel: True
+    smoother_plugins: ['simple_smoother']
+    simple_smoother:
+      plugin: 'nav2_smoother::SimpleSmoother'
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: true
+
+behavior_server:
+  ros__parameters:
+    use_sim_time: true
+    enable_stamped_cmd_vel: True
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ['spin', 'backup', 'drive_on_heading', 'assisted_teleop', 'wait']
+    spin:
+      plugin: 'nav2_behaviors::Spin'
+    backup:
+      plugin: 'nav2_behaviors::BackUp'
+    drive_on_heading:
+      plugin: 'nav2_behaviors::DriveOnHeading'
+    wait:
+      plugin: 'nav2_behaviors::Wait'
+    assisted_teleop:
+      plugin: 'nav2_behaviors::AssistedTeleop'
+    local_frame: odom
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.1
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: true
+    enable_stamped_cmd_vel: True
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: 'OPEN_LOOP'
+    max_velocity: [0.26, 0.0, 1.0]
+    min_velocity: [-0.26, 0.0, -1.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: /fusion/odom
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    use_sim_time: true
+    enable_stamped_cmd_vel: True
+    base_frame_id: base_footprint
+    odom_frame_id: odom
+    cmd_vel_in_topic: cmd_vel_smoothed
+    cmd_vel_out_topic: cmd_vel
+    state_topic: collision_monitor_state
+    transform_error_pub_topic: transform_error
+    polygons: ['FootprintApproach']
+    FootprintApproach:
+      type: polygon
+      action_type: approach
+      footprint_topic: local_costmap/published_footprint
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: false
+      enabled: true
+    observation_sources: ['scan']
+    scan:
+      type: scan
+      topic: /scan
+      min_height: 0.15
+      max_height: 2.0
+      enabled: true
+
+
+docking_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_footprint"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3

rtabmap_demos/params/turtlebot3_nav2_params.yaml

@@ -0,0 +1,421 @@
+bt_navigator:
+  ros__parameters:
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+
+    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+    # Built-in plugins are added automatically
+    # plugin_lib_names: []
+
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
+
+controller_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    controller_frequency: 20.0
+    costmap_update_timeout: 0.30
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugins: ["progress_checker"]
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.70
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.7
+      always_send_full_costmap: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
+    GridBased:
+      plugin: "nav2_navfn_planner::NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors::Spin"
+    backup:
+      plugin: "nav2_behaviors::BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors::DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors::Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors::AssistedTeleop"
+    local_frame: odom
+    global_frame: map
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    action_server_result_timeout: 900.0
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+route_server:
+  ros__parameters:
+    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
+    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
+    # file & provide full path to map below. If graph config or launch default is provided, it is used
+    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    smooth_corners: true
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 3.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
+velocity_smoother:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.0, 2.0]
+    min_velocity: [-0.5, 0.0, -2.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
+    # and robot footprint for "approach" action type.
+    polygons: ["FootprintApproach"]
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+docking_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true

rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml

@@ -1,85 +1,44 @@
 # rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source.
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
     global_frame: map
     robot_base_frame: base_link
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
     wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
-    plugin_lib_names:
-      - nav2_compute_path_to_pose_action_bt_node
-      - nav2_compute_path_through_poses_action_bt_node
-      - nav2_smooth_path_action_bt_node
-      - nav2_follow_path_action_bt_node
-      - nav2_spin_action_bt_node
-      - nav2_wait_action_bt_node
-      - nav2_assisted_teleop_action_bt_node
-      - nav2_back_up_action_bt_node
-      - nav2_drive_on_heading_bt_node
-      - nav2_clear_costmap_service_bt_node
-      - nav2_is_stuck_condition_bt_node
-      - nav2_goal_reached_condition_bt_node
-      - nav2_goal_updated_condition_bt_node
-      - nav2_globally_updated_goal_condition_bt_node
-      - nav2_is_path_valid_condition_bt_node
-      - nav2_initial_pose_received_condition_bt_node
-      - nav2_reinitialize_global_localization_service_bt_node
-      - nav2_rate_controller_bt_node
-      - nav2_distance_controller_bt_node
-      - nav2_speed_controller_bt_node
-      - nav2_truncate_path_action_bt_node
-      - nav2_truncate_path_local_action_bt_node
-      - nav2_goal_updater_node_bt_node
-      - nav2_recovery_node_bt_node
-      - nav2_pipeline_sequence_bt_node
-      - nav2_round_robin_node_bt_node
-      - nav2_transform_available_condition_bt_node
-      - nav2_time_expired_condition_bt_node
-      - nav2_path_expiring_timer_condition
-      - nav2_distance_traveled_condition_bt_node
-      - nav2_single_trigger_bt_node
-      - nav2_goal_updated_controller_bt_node
-      - nav2_is_battery_low_condition_bt_node
-      - nav2_navigate_through_poses_action_bt_node
-      - nav2_navigate_to_pose_action_bt_node
-      - nav2_remove_passed_goals_action_bt_node
-      - nav2_planner_selector_bt_node
-      - nav2_controller_selector_bt_node
-      - nav2_goal_checker_selector_bt_node
-      - nav2_controller_cancel_bt_node
-      - nav2_path_longer_on_approach_bt_node
-      - nav2_wait_cancel_bt_node
-      - nav2_spin_cancel_bt_node
-      - nav2_back_up_cancel_bt_node
-      - nav2_assisted_teleop_cancel_bt_node
-      - nav2_drive_on_heading_cancel_bt_node
-      - nav2_is_battery_charging_condition_bt_node
 
-bt_navigator_navigate_through_poses_rclcpp_node:
-  ros__parameters:
-    use_sim_time: True
+    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+    # Built-in plugins are added automatically
+    # plugin_lib_names: []
 
-bt_navigator_navigate_to_pose_rclcpp_node:
-  ros__parameters:
-    use_sim_time: True
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    enable_stamped_cmd_vel: True
     controller_frequency: 20.0
+    costmap_update_timeout: 0.30
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
 
     # Progress checker parameters
     progress_checker:
@@ -97,48 +56,96 @@ controller_server:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.25
       yaw_goal_tolerance: 0.25
-    # DWB parameters
     FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -147,7 +154,6 @@ local_costmap:
       publish_frequency: 2.0
       global_frame: odom
       robot_base_frame: base_link
-      use_sim_time: True
       rolling_window: true
       width: 3
       height: 3
@@ -157,7 +163,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.70
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -200,7 +206,6 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
       robot_radius: 0.22
       resolution: 0.05
       track_unknown_space: true
@@ -211,19 +216,22 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.7
       always_send_full_costmap: True
 
-map_server:
+planner_server:
   ros__parameters:
-    use_sim_time: True
-    # Overridden in launch by the "map" launch configuration or provided default value.
-    # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
-    yaml_filename: ""
+    expected_planner_frequency: 20.0
+    planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
+    GridBased:
+      plugin: "nav2_navfn_planner::NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
 
 smoother_server:
   ros__parameters:
-    use_sim_time: True
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -233,55 +241,178 @@ smoother_server:
 
 behavior_server:
   ros__parameters:
-    costmap_topic: local_costmap/costmap_raw
-    footprint_topic: local_costmap/published_footprint
+    enable_stamped_cmd_vel: True
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
     behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
-      plugin: "nav2_behaviors/Spin"
+      plugin: "nav2_behaviors::Spin"
     backup:
-      plugin: "nav2_behaviors/BackUp"
+      plugin: "nav2_behaviors::BackUp"
     drive_on_heading:
-      plugin: "nav2_behaviors/DriveOnHeading"
+      plugin: "nav2_behaviors::DriveOnHeading"
     wait:
-      plugin: "nav2_behaviors/Wait"
+      plugin: "nav2_behaviors::Wait"
     assisted_teleop:
-      plugin: "nav2_behaviors/AssistedTeleop"
-    global_frame: odom
+      plugin: "nav2_behaviors::AssistedTeleop"
+    local_frame: odom
+    global_frame: map
     robot_base_frame: base_link
     transform_tolerance: 0.1
-    use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
     rotational_acc_lim: 3.2
 
-robot_state_publisher:
-  ros__parameters:
-    use_sim_time: True
-
 waypoint_follower:
   ros__parameters:
-    use_sim_time: True
     loop_rate: 20
     stop_on_failure: false
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
       waypoint_pause_duration: 200
 
+route_server:
+  ros__parameters:
+    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
+    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
+    # file & provide full path to map below. If graph config or launch default is provided, it is used
+    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    smooth_corners: true
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 3.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
 velocity_smoother:
   ros__parameters:
-    use_sim_time: True
+    enable_stamped_cmd_vel: True
     smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.26, 0.0, 1.0]
-    min_velocity: [-0.26, 0.0, -1.0]
+    max_velocity: [0.5, 0.0, 2.0]
+    min_velocity: [-0.5, 0.0, -2.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
+    # and robot footprint for "approach" action type.
+    polygons: ["FootprintApproach"]
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+docking_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true

rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml

@@ -1,85 +1,44 @@
 # rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap.
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
     global_frame: map
     robot_base_frame: base_link
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
     wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
-    plugin_lib_names:
-      - nav2_compute_path_to_pose_action_bt_node
-      - nav2_compute_path_through_poses_action_bt_node
-      - nav2_smooth_path_action_bt_node
-      - nav2_follow_path_action_bt_node
-      - nav2_spin_action_bt_node
-      - nav2_wait_action_bt_node
-      - nav2_assisted_teleop_action_bt_node
-      - nav2_back_up_action_bt_node
-      - nav2_drive_on_heading_bt_node
-      - nav2_clear_costmap_service_bt_node
-      - nav2_is_stuck_condition_bt_node
-      - nav2_goal_reached_condition_bt_node
-      - nav2_goal_updated_condition_bt_node
-      - nav2_globally_updated_goal_condition_bt_node
-      - nav2_is_path_valid_condition_bt_node
-      - nav2_initial_pose_received_condition_bt_node
-      - nav2_reinitialize_global_localization_service_bt_node
-      - nav2_rate_controller_bt_node
-      - nav2_distance_controller_bt_node
-      - nav2_speed_controller_bt_node
-      - nav2_truncate_path_action_bt_node
-      - nav2_truncate_path_local_action_bt_node
-      - nav2_goal_updater_node_bt_node
-      - nav2_recovery_node_bt_node
-      - nav2_pipeline_sequence_bt_node
-      - nav2_round_robin_node_bt_node
-      - nav2_transform_available_condition_bt_node
-      - nav2_time_expired_condition_bt_node
-      - nav2_path_expiring_timer_condition
-      - nav2_distance_traveled_condition_bt_node
-      - nav2_single_trigger_bt_node
-      - nav2_goal_updated_controller_bt_node
-      - nav2_is_battery_low_condition_bt_node
-      - nav2_navigate_through_poses_action_bt_node
-      - nav2_navigate_to_pose_action_bt_node
-      - nav2_remove_passed_goals_action_bt_node
-      - nav2_planner_selector_bt_node
-      - nav2_controller_selector_bt_node
-      - nav2_goal_checker_selector_bt_node
-      - nav2_controller_cancel_bt_node
-      - nav2_path_longer_on_approach_bt_node
-      - nav2_wait_cancel_bt_node
-      - nav2_spin_cancel_bt_node
-      - nav2_back_up_cancel_bt_node
-      - nav2_assisted_teleop_cancel_bt_node
-      - nav2_drive_on_heading_cancel_bt_node
-      - nav2_is_battery_charging_condition_bt_node
 
-bt_navigator_navigate_through_poses_rclcpp_node:
-  ros__parameters:
-    use_sim_time: True
+    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+    # Built-in plugins are added automatically
+    # plugin_lib_names: []
 
-bt_navigator_navigate_to_pose_rclcpp_node:
-  ros__parameters:
-    use_sim_time: True
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    enable_stamped_cmd_vel: True
     controller_frequency: 20.0
+    costmap_update_timeout: 0.30
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
 
     # Progress checker parameters
     progress_checker:
@@ -97,48 +56,96 @@ controller_server:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.25
       yaw_goal_tolerance: 0.25
-    # DWB parameters
     FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -147,7 +154,6 @@ local_costmap:
       publish_frequency: 2.0
       global_frame: odom
       robot_base_frame: base_link
-      use_sim_time: True
       rolling_window: true
       width: 3
       height: 3
@@ -157,7 +163,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.70
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -210,7 +216,6 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
       robot_radius: 0.22
       resolution: 0.05
       track_unknown_space: true
@@ -221,23 +226,22 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.7
       always_send_full_costmap: True
 
 planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
-    use_sim_time: True
     planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
     GridBased:
-      plugin: "nav2_navfn_planner/NavfnPlanner"
+      plugin: "nav2_navfn_planner::NavfnPlanner"
       tolerance: 0.5
       use_astar: false
       allow_unknown: true
 
 smoother_server:
   ros__parameters:
-    use_sim_time: True
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -247,55 +251,178 @@ smoother_server:
 
 behavior_server:
   ros__parameters:
-    costmap_topic: local_costmap/costmap_raw
-    footprint_topic: local_costmap/published_footprint
+    enable_stamped_cmd_vel: True
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
     behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
-      plugin: "nav2_behaviors/Spin"
+      plugin: "nav2_behaviors::Spin"
     backup:
-      plugin: "nav2_behaviors/BackUp"
+      plugin: "nav2_behaviors::BackUp"
     drive_on_heading:
-      plugin: "nav2_behaviors/DriveOnHeading"
+      plugin: "nav2_behaviors::DriveOnHeading"
     wait:
-      plugin: "nav2_behaviors/Wait"
+      plugin: "nav2_behaviors::Wait"
     assisted_teleop:
-      plugin: "nav2_behaviors/AssistedTeleop"
-    global_frame: odom
+      plugin: "nav2_behaviors::AssistedTeleop"
+    local_frame: odom
+    global_frame: map
     robot_base_frame: base_link
     transform_tolerance: 0.1
-    use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
     rotational_acc_lim: 3.2
 
-robot_state_publisher:
-  ros__parameters:
-    use_sim_time: True
-
 waypoint_follower:
   ros__parameters:
-    use_sim_time: True
     loop_rate: 20
     stop_on_failure: false
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
       waypoint_pause_duration: 200
 
+route_server:
+  ros__parameters:
+    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
+    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
+    # file & provide full path to map below. If graph config or launch default is provided, it is used
+    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    smooth_corners: true
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 3.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
 velocity_smoother:
   ros__parameters:
-    use_sim_time: True
+    enable_stamped_cmd_vel: True
     smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.26, 0.0, 1.0]
-    min_velocity: [-0.26, 0.0, -1.0]
+    max_velocity: [0.5, 0.0, 2.0]
+    min_velocity: [-0.5, 0.0, -2.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
+    # and robot footprint for "approach" action type.
+    polygons: ["FootprintApproach"]
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+docking_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true

rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml

@@ -1,85 +1,44 @@
-# Modified to use icp_odom frame
+# Using icp_odom TF instead of odom
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
     global_frame: map
     robot_base_frame: base_link
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
     wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
-    plugin_lib_names:
-      - nav2_compute_path_to_pose_action_bt_node
-      - nav2_compute_path_through_poses_action_bt_node
-      - nav2_smooth_path_action_bt_node
-      - nav2_follow_path_action_bt_node
-      - nav2_spin_action_bt_node
-      - nav2_wait_action_bt_node
-      - nav2_assisted_teleop_action_bt_node
-      - nav2_back_up_action_bt_node
-      - nav2_drive_on_heading_bt_node
-      - nav2_clear_costmap_service_bt_node
-      - nav2_is_stuck_condition_bt_node
-      - nav2_goal_reached_condition_bt_node
-      - nav2_goal_updated_condition_bt_node
-      - nav2_globally_updated_goal_condition_bt_node
-      - nav2_is_path_valid_condition_bt_node
-      - nav2_initial_pose_received_condition_bt_node
-      - nav2_reinitialize_global_localization_service_bt_node
-      - nav2_rate_controller_bt_node
-      - nav2_distance_controller_bt_node
-      - nav2_speed_controller_bt_node
-      - nav2_truncate_path_action_bt_node
-      - nav2_truncate_path_local_action_bt_node
-      - nav2_goal_updater_node_bt_node
-      - nav2_recovery_node_bt_node
-      - nav2_pipeline_sequence_bt_node
-      - nav2_round_robin_node_bt_node
-      - nav2_transform_available_condition_bt_node
-      - nav2_time_expired_condition_bt_node
-      - nav2_path_expiring_timer_condition
-      - nav2_distance_traveled_condition_bt_node
-      - nav2_single_trigger_bt_node
-      - nav2_goal_updated_controller_bt_node
-      - nav2_is_battery_low_condition_bt_node
-      - nav2_navigate_through_poses_action_bt_node
-      - nav2_navigate_to_pose_action_bt_node
-      - nav2_remove_passed_goals_action_bt_node
-      - nav2_planner_selector_bt_node
-      - nav2_controller_selector_bt_node
-      - nav2_goal_checker_selector_bt_node
-      - nav2_controller_cancel_bt_node
-      - nav2_path_longer_on_approach_bt_node
-      - nav2_wait_cancel_bt_node
-      - nav2_spin_cancel_bt_node
-      - nav2_back_up_cancel_bt_node
-      - nav2_assisted_teleop_cancel_bt_node
-      - nav2_drive_on_heading_cancel_bt_node
-      - nav2_is_battery_charging_condition_bt_node
 
-bt_navigator_navigate_through_poses_rclcpp_node:
-  ros__parameters:
-    use_sim_time: True
+    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+    # Built-in plugins are added automatically
+    # plugin_lib_names: []
 
-bt_navigator_navigate_to_pose_rclcpp_node:
-  ros__parameters:
-    use_sim_time: True
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    enable_stamped_cmd_vel: True
     controller_frequency: 20.0
+    costmap_update_timeout: 0.30
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
-    progress_checker_plugin: "progress_checker"
+    progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
 
     # Progress checker parameters
     progress_checker:
@@ -97,48 +56,96 @@ controller_server:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.25
       yaw_goal_tolerance: 0.25
-    # DWB parameters
     FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -147,7 +154,6 @@ local_costmap:
       publish_frequency: 2.0
       global_frame: icp_odom
       robot_base_frame: base_link
-      use_sim_time: True
       rolling_window: true
       width: 3
       height: 3
@@ -157,7 +163,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.70
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -190,7 +196,6 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
       robot_radius: 0.22
       resolution: 0.05
       track_unknown_space: true
@@ -215,23 +220,22 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.7
       always_send_full_costmap: True
 
 planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
-    use_sim_time: True
     planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
     GridBased:
-      plugin: "nav2_navfn_planner/NavfnPlanner"
+      plugin: "nav2_navfn_planner::NavfnPlanner"
       tolerance: 0.5
       use_astar: false
       allow_unknown: true
 
 smoother_server:
   ros__parameters:
-    use_sim_time: True
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -241,55 +245,178 @@ smoother_server:
 
 behavior_server:
   ros__parameters:
-    costmap_topic: local_costmap/costmap_raw
-    footprint_topic: local_costmap/published_footprint
+    enable_stamped_cmd_vel: True
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 10.0
     behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
-      plugin: "nav2_behaviors/Spin"
+      plugin: "nav2_behaviors::Spin"
     backup:
-      plugin: "nav2_behaviors/BackUp"
+      plugin: "nav2_behaviors::BackUp"
     drive_on_heading:
-      plugin: "nav2_behaviors/DriveOnHeading"
+      plugin: "nav2_behaviors::DriveOnHeading"
     wait:
-      plugin: "nav2_behaviors/Wait"
+      plugin: "nav2_behaviors::Wait"
     assisted_teleop:
-      plugin: "nav2_behaviors/AssistedTeleop"
-    global_frame: icp_odom
+      plugin: "nav2_behaviors::AssistedTeleop"
+    local_frame: icp_odom
+    global_frame: map
     robot_base_frame: base_link
     transform_tolerance: 0.1
-    use_sim_time: true
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
     rotational_acc_lim: 3.2
 
-robot_state_publisher:
-  ros__parameters:
-    use_sim_time: True
-
 waypoint_follower:
   ros__parameters:
-    use_sim_time: True
     loop_rate: 20
     stop_on_failure: false
+    action_server_result_timeout: 900.0
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
       waypoint_pause_duration: 200
 
+route_server:
+  ros__parameters:
+    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
+    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
+    # file & provide full path to map below. If graph config or launch default is provided, it is used
+    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    smooth_corners: true
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 3.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
 velocity_smoother:
   ros__parameters:
-    use_sim_time: True
+    enable_stamped_cmd_vel: True
     smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.26, 0.0, 1.0]
-    min_velocity: [-0.26, 0.0, -1.0]
+    max_velocity: [0.5, 0.0, 2.0]
+    min_velocity: [-0.5, 0.0, -2.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    base_frame_id: "base_footprint"
+    odom_frame_id: "icp_odom"
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
+    # and robot footprint for "approach" action type.
+    polygons: ["FootprintApproach"]
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+docking_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: True
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "icp_odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "icp_odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true

rtabmap_examples/config/multi_rgbd_inertial_dataset.ini

@@ -0,0 +1,453 @@
+[Core]
+Rtabmap\WorkingDirectory=/home/vscode/.ros
+
+[Gui]
+AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10\0\0\0\0\0\0\0\0\a\x80\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10)
+DepthCalibrationDialog\bin_depth=2
+DepthCalibrationDialog\bin_height=6
+DepthCalibrationDialog\bin_width=8
+DepthCalibrationDialog\cone_radius=0.02
+DepthCalibrationDialog\cone_stddev_thresh=0.1
+DepthCalibrationDialog\decimation=1
+DepthCalibrationDialog\laser_scan=false
+DepthCalibrationDialog\max_depth=3.5
+DepthCalibrationDialog\max_model_depth=10
+DepthCalibrationDialog\min_depth=0
+DepthCalibrationDialog\smoothing=1
+DepthCalibrationDialog\voxel=0.01
+ExportBundlerDialog\exportPoints=false
+ExportBundlerDialog\laplacianThr=0
+ExportBundlerDialog\maxAngularSpeed=0
+ExportBundlerDialog\maxLinearSpeed=0
+ExportBundlerDialog\sba_iterations=20
+ExportBundlerDialog\sba_rematch_features=true
+ExportBundlerDialog\sba_type=0
+ExportBundlerDialog\sba_variance=1
+ExportCloudsDialog\assemble=true
+ExportCloudsDialog\assemble_samples=0
+ExportCloudsDialog\assemble_voxel=0.01
+ExportCloudsDialog\bilateral=false
+ExportCloudsDialog\bilateral_sigma_r=0.1
+ExportCloudsDialog\bilateral_sigma_s=10
+ExportCloudsDialog\binary=true
+ExportCloudsDialog\cam_proj=false
+ExportCloudsDialog\cam_proj_decimation=1
+ExportCloudsDialog\cam_proj_distance_policy=true
+ExportCloudsDialog\cam_proj_export_format=0
+ExportCloudsDialog\cam_proj_keep_points=false
+ExportCloudsDialog\cam_proj_mask=
+ExportCloudsDialog\cam_proj_max_angle=0
+ExportCloudsDialog\cam_proj_max_depth_error=0
+ExportCloudsDialog\cam_proj_max_distance=0
+ExportCloudsDialog\cam_proj_recolor_points=true
+ExportCloudsDialog\cam_proj_roi_ratios=0.0 0.0 0.0 0.0
+ExportCloudsDialog\cputsdf_flattenRadius=0.005
+ExportCloudsDialog\cputsdf_minWeight=0
+ExportCloudsDialog\cputsdf_randomSplit=1
+ExportCloudsDialog\cputsdf_resolution=0.01
+ExportCloudsDialog\cputsdf_size=12
+ExportCloudsDialog\cputsdf_truncNeg=0.03
+ExportCloudsDialog\cputsdf_truncPos=0.03
+ExportCloudsDialog\filtering=false
+ExportCloudsDialog\filtering_min_neighbors=5
+ExportCloudsDialog\filtering_radius=0
+ExportCloudsDialog\frame=0
+ExportCloudsDialog\from_depth=true
+ExportCloudsDialog\gain=false
+ExportCloudsDialog\gain_beta=10
+ExportCloudsDialog\gain_full=false
+ExportCloudsDialog\gain_overlap=0
+ExportCloudsDialog\gain_radius=0.02
+ExportCloudsDialog\gain_rgb=true
+ExportCloudsDialog\intensity_colormap=0
+ExportCloudsDialog\mesh=false
+ExportCloudsDialog\mesh_angle_tolerance=15
+ExportCloudsDialog\mesh_clean=true
+ExportCloudsDialog\mesh_color_radius=0.05
+ExportCloudsDialog\mesh_decimation_factor=0
+ExportCloudsDialog\mesh_dense_strategy=1
+ExportCloudsDialog\mesh_max_polygons=0
+ExportCloudsDialog\mesh_min_cluster_size=0
+ExportCloudsDialog\mesh_mu=2.5
+ExportCloudsDialog\mesh_quad=false
+ExportCloudsDialog\mesh_radius=0.2
+ExportCloudsDialog\mesh_texture=false
+ExportCloudsDialog\mesh_textureBlending=true
+ExportCloudsDialog\mesh_textureBlendingDecimation=0
+ExportCloudsDialog\mesh_textureBrightnessConstrastRatioHigh=5
+ExportCloudsDialog\mesh_textureBrightnessConstrastRatioLow=0
+ExportCloudsDialog\mesh_textureCameraFiltering=false
+ExportCloudsDialog\mesh_textureCameraFilteringAngle=30
+ExportCloudsDialog\mesh_textureCameraFilteringLaplacian=0
+ExportCloudsDialog\mesh_textureCameraFilteringRadius=0
+ExportCloudsDialog\mesh_textureCameraFilteringVel=0
+ExportCloudsDialog\mesh_textureCameraFilteringVelRad=0
+ExportCloudsDialog\mesh_textureDistanceToCamPolicy=false
+ExportCloudsDialog\mesh_textureExposureFusion=false
+ExportCloudsDialog\mesh_textureFormat=0
+ExportCloudsDialog\mesh_textureMaxAngle=0
+ExportCloudsDialog\mesh_textureMaxCount=1
+ExportCloudsDialog\mesh_textureMaxDepthError=0
+ExportCloudsDialog\mesh_textureMaxDistance=3
+ExportCloudsDialog\mesh_textureMinCluster=50
+ExportCloudsDialog\mesh_textureMultiband=false
+ExportCloudsDialog\mesh_textureMultibandAngleHardThr=90
+ExportCloudsDialog\mesh_textureMultibandBestScoreThr=0.1
+ExportCloudsDialog\mesh_textureMultibandDownScale=2
+ExportCloudsDialog\mesh_textureMultibandFillHoles=false
+ExportCloudsDialog\mesh_textureMultibandForceVisible=false
+ExportCloudsDialog\mesh_textureMultibandNbContrib=1 5 10 0
+ExportCloudsDialog\mesh_textureMultibandPadding=5
+ExportCloudsDialog\mesh_textureMultibandUnwrap=0
+ExportCloudsDialog\mesh_textureRoiRatios=0.0 0.0 0.0 0.0
+ExportCloudsDialog\mesh_textureSize=6
+ExportCloudsDialog\mesh_textureVertexColorPolicy=0
+ExportCloudsDialog\mesh_triangle_size=1
+ExportCloudsDialog\mls=false
+ExportCloudsDialog\mls_dilation_iterations=1
+ExportCloudsDialog\mls_dilation_voxel_size=0.005
+ExportCloudsDialog\mls_output_voxel_size=0
+ExportCloudsDialog\mls_point_density=10
+ExportCloudsDialog\mls_polygonial_order=2
+ExportCloudsDialog\mls_radius=0.04
+ExportCloudsDialog\mls_upsampling_method=0
+ExportCloudsDialog\mls_upsampling_radius=0.01
+ExportCloudsDialog\mls_upsampling_step=0.005
+ExportCloudsDialog\nodes_filtering=false
+ExportCloudsDialog\nodes_filtering_xmax=0
+ExportCloudsDialog\nodes_filtering_xmin=0
+ExportCloudsDialog\nodes_filtering_ymax=0
+ExportCloudsDialog\nodes_filtering_ymin=0
+ExportCloudsDialog\nodes_filtering_zmax=0
+ExportCloudsDialog\nodes_filtering_zmin=0
+ExportCloudsDialog\normals_ground_normals_up=0
+ExportCloudsDialog\normals_k=20
+ExportCloudsDialog\normals_radius=0
+ExportCloudsDialog\openchisel_carving_dist_m=0.05
+ExportCloudsDialog\openchisel_chunk_size_x=16
+ExportCloudsDialog\openchisel_chunk_size_y=16
+ExportCloudsDialog\openchisel_chunk_size_z=16
+ExportCloudsDialog\openchisel_far_plane_dist=1.1
+ExportCloudsDialog\openchisel_integration_weight=1
+ExportCloudsDialog\openchisel_merge_vertices=true
+ExportCloudsDialog\openchisel_near_plane_dist=0.05
+ExportCloudsDialog\openchisel_truncation_constant=0.001504
+ExportCloudsDialog\openchisel_truncation_linear=0.00152
+ExportCloudsDialog\openchisel_truncation_quadratic=0.0019
+ExportCloudsDialog\openchisel_truncation_scale=10
+ExportCloudsDialog\openchisel_use_voxel_carving=false
+ExportCloudsDialog\pipeline=1
+ExportCloudsDialog\poisson_depth=0
+ExportCloudsDialog\poisson_iso=8
+ExportCloudsDialog\poisson_manifold=true
+ExportCloudsDialog\poisson_minDepth=5
+ExportCloudsDialog\poisson_outputPolygons=false
+ExportCloudsDialog\poisson_pointWeight=4
+ExportCloudsDialog\poisson_polygon_size=0.03
+ExportCloudsDialog\poisson_samples=1
+ExportCloudsDialog\poisson_scale=1.1
+ExportCloudsDialog\poisson_solver=8
+ExportCloudsDialog\regenerate=false
+ExportCloudsDialog\regenerate_ceiling=0
+ExportCloudsDialog\regenerate_decimation=1
+ExportCloudsDialog\regenerate_distortion_model=
+ExportCloudsDialog\regenerate_edge_bleeding_error=0
+ExportCloudsDialog\regenerate_fill_error=2
+ExportCloudsDialog\regenerate_fill_size=0
+ExportCloudsDialog\regenerate_floor=0
+ExportCloudsDialog\regenerate_footprint_height=0
+ExportCloudsDialog\regenerate_footprint_length=0
+ExportCloudsDialog\regenerate_footprint_width=0
+ExportCloudsDialog\regenerate_max_depth=4
+ExportCloudsDialog\regenerate_min_depth=0
+ExportCloudsDialog\regenerate_min_depth_confidence=0
+ExportCloudsDialog\regenerate_offaxis_filtering=false
+ExportCloudsDialog\regenerate_offaxis_filtering_angle=10
+ExportCloudsDialog\regenerate_offaxis_filtering_neg_x=true
+ExportCloudsDialog\regenerate_offaxis_filtering_neg_y=true
+ExportCloudsDialog\regenerate_offaxis_filtering_neg_z=true
+ExportCloudsDialog\regenerate_offaxis_filtering_pos_x=true
+ExportCloudsDialog\regenerate_offaxis_filtering_pos_y=true
+ExportCloudsDialog\regenerate_offaxis_filtering_pos_z=true
+ExportCloudsDialog\regenerate_roi=0.0 0.0 0.0 0.0
+ExportCloudsDialog\regenerate_scan_decimation=1
+ExportCloudsDialog\regenerate_scan_max_range=0
+ExportCloudsDialog\regenerate_scan_min_range=0
+ExportCloudsDialog\subtract=false
+ExportCloudsDialog\subtract_min_neighbors=5
+ExportCloudsDialog\subtract_point_angle=0
+ExportCloudsDialog\subtract_point_radius=0.02
+Figures\counts=
+Figures\curves=
+Figures\thresholds=
+General\beep=false
+General\cloudCeilingHeight=0
+General\cloudFiltering=false
+General\cloudFilteringAngle=30
+General\cloudFilteringRadius=0.1
+General\cloudFloorHeight=0
+General\cloudNoiseMinNeighbors=5
+General\cloudNoiseRadius=0
+General\cloudVoxel=0
+General\cloudsKept=true
+General\colorScheme0=0
+General\colorScheme1=0
+General\colorSchemeScan0=0
+General\colorSchemeScan1=0
+General\decimation0=8
+General\decimation1=4
+General\depthConf0=0
+General\depthConf1=0
+General\downsamplingScan0=1
+General\downsamplingScan1=1
+General\elevationMapShown=0
+General\figure_cache=true
+General\figure_time=true
+General\gravityLength0=1
+General\gravityLength1=1
+General\gravityShown0=false
+General\gravityShown1=true
+General\gridMapOpacity=0.75
+General\gridMapShown=false
+General\gridUIResolution=0
+General\gtAlign=true
+General\imageHighestHypShown=false
+General\imageRejectedShown=true
+General\imagesKept=true
+General\landmarkSize=0
+General\localizationsGraphView=false
+General\localizationsGraphViewOdomCache=false
+General\loggerEventLevel=3
+General\loggerLevel=2
+General\loggerPauseLevel=3
+General\loggerPrintThreadId=false
+General\loggerPrintTime=true
+General\loggerType=1
+General\maxDepth0=5
+General\maxDepth1=0
+General\maxRange0=0
+General\maxRange1=0
+General\meshing=false
+General\meshing_angle=15
+General\meshing_quad=true
+General\meshing_texture=false
+General\meshing_triangle_size=2
+General\minDepth0=0
+General\minDepth1=0
+General\minRange0=0
+General\minRange1=0
+General\missingRepublished=true
+General\noFiltering=true
+General\nochangeGraphView=false
+General\normalKSearch=10
+General\normalRadiusSearch=0
+General\notifyNewGlobalPath=false
+General\octomap=false
+General\octomap_2dgrid=true
+General\octomap_3dmap=true
+General\octomap_depth=16
+General\octomap_point_size=5
+General\octomap_rendering_type=0
+General\odomDisabled=false
+General\odomF2MGravitySigma=-1
+General\odomOnlyInliersShown=false
+General\odomQualityThr=50
+General\odomRegistration=3
+General\opacity0=1
+General\opacity1=0.75
+General\opacityScan0=1
+General\opacityScan1=0.5
+General\posteriorGraphView=true
+General\ptSize0=1
+General\ptSize1=2
+General\ptSizeFeatures0=3
+General\ptSizeFeatures1=3
+General\ptSizeScan0=1
+General\ptSizeScan1=2
+General\roiRatios0=0.0 0.0 0.0 0.0
+General\roiRatios1=0.0 0.0 0.0 0.0
+General\scanCeilingHeight=0
+General\scanFloorHeight=0
+General\scanNormalKSearch=0
+General\scanNormalRadiusSearch=0
+General\showClouds0=true
+General\showClouds1=false
+General\showFeatures0=false
+General\showFeatures1=true
+General\showFrames=false
+General\showFrustums0=false
+General\showFrustums1=false
+General\showGraphs=true
+General\showIMUAcc=false
+General\showIMUGravity=false
+General\showLabels=false
+General\showLandmarks=true
+General\showScans0=true
+General\showScans1=true
+General\subtractFiltering=false
+General\subtractFilteringAngle=0
+General\subtractFilteringMinPts=5
+General\subtractFilteringRadius=0.02
+General\verticalLayoutUsed=true
+General\voxelSizeScan0=0
+General\voxelSizeScan1=0
+General\wordsGraphView=false
+MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\x32\0\0\0\x1b\0\0\x6\xf2\0\0\x3\xf\0\0\0\x32\0\0\0@\0\0\x6\xf2\0\0\x3\xf\0\0\0\0\0\0\0\0\a\x80\0\0\0\x32\0\0\0@\0\0\x6\xf2\0\0\x3\xf)
+MainWindow\maximized=false
+MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x3\\\0\0\x2\x95\xfc\x2\0\0\0\x3\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x2\0\0\0\x32\0\0\0\x1b\0\0\0\xc8\0\0\0\xc1\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0;\0\0\x1\xaa\0\0\0\x37\0\xff\xff\xff\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\x1\xeb\0\0\0\xe5\0\0\0\x13\0\xff\xff\xff\0\0\0\x1\0\0\x3_\0\0\x2\x95\xfc\x2\0\0\0\x4\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0;\0\0\x2\x95\0\0\0\xdb\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf0\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x2\0\0\x4\xf2\0\0\x1\x39\0\0\x2}\0\0\x1\x90\xfb\0\0\0\x34\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0u\0l\0t\0i\0S\0\x65\0s\0s\0i\0o\0n\0L\0o\0\x63\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x13\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\x1(\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\x2\0\0\0\x32\0\0\0\x1b\0\0\0\xc8\0\0\0\x88\0\0\0\0\0\0\x2\x95\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
+MainWindow\status_bar=false
+PostProcessingDialog\cluster_angle=30
+PostProcessingDialog\cluster_radius=1
+PostProcessingDialog\detect_more_lc=true
+PostProcessingDialog\inter_session=true
+PostProcessingDialog\intra_session=true
+PostProcessingDialog\iterations=5
+PostProcessingDialog\refine_lc=false
+PostProcessingDialog\refine_neigbors=false
+PostProcessingDialog\sba=false
+PostProcessingDialog\sba_iterations=20
+PostProcessingDialog\sba_rematch_features=true
+PostProcessingDialog\sba_type=1
+PostProcessingDialog\sba_variance=1
+PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x1\xa8\xff\xff\xff\xf6\0\0\x5{\0\0\x3\xb7\0\0\x1\xa8\0\0\0\x1b\0\0\x5{\0\0\x3\xb7\0\0\0\0\0\0\0\0\a\x80\0\0\x1\xa8\0\0\0\x1b\0\0\x5{\0\0\x3\xb7)
+graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
+graphicsView_graphView\ensure_frame_visible=1
+graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
+graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
+graphicsView_graphView\global_path_visible=true
+graphicsView_graphView\gps_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\x80\x80\0\0)
+graphicsView_graphView\gps_graph_visible=true
+graphicsView_graphView\graph_visible=true
+graphicsView_graphView\grid_visible=true
+graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
+graphicsView_graphView\gt_graph_visible=true
+graphicsView_graphView\highlighting_color_0=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+graphicsView_graphView\highlighting_color_1=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0)
+graphicsView_graphView\intra_inter_session_colors_enabled=false
+graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
+graphicsView_graphView\link_width=0
+graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+graphicsView_graphView\local_path_visible=true
+graphicsView_graphView\local_radius_visible=false
+graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0)
+graphicsView_graphView\min_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n)
+graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
+graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0)
+graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
+graphicsView_graphView\node_odom_cache_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\0\0\0\0)
+graphicsView_graphView\node_radius=0.009999999776482582
+graphicsView_graphView\node_visible=true
+graphicsView_graphView\odom_cache_overlay=true
+graphicsView_graphView\orientation_ENU=false
+graphicsView_graphView\origin_visible=true
+graphicsView_graphView\referential_visible=true
+graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
+graphicsView_graphView\view_plane=0
+graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_loopClosure\alpha=100
+imageView_loopClosure\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_loopClosure\colormap=2
+imageView_loopClosure\colormap_camera_frame=true
+imageView_loopClosure\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0)
+imageView_loopClosure\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0)
+imageView_loopClosure\confidence_shown=false
+imageView_loopClosure\depth_shown=false
+imageView_loopClosure\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+imageView_loopClosure\features_shown=true
+imageView_loopClosure\features_size=0
+imageView_loopClosure\graphics_view=false
+imageView_loopClosure\graphics_view_scale=true
+imageView_loopClosure\graphics_view_scale_to_height=false
+imageView_loopClosure\image_shown=true
+imageView_loopClosure\lines_shown=true
+imageView_loopClosure\lines_width=0
+imageView_loopClosure\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_loopClosure\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+imageView_odometry\alpha=200
+imageView_odometry\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_odometry\colormap=2
+imageView_odometry\colormap_camera_frame=true
+imageView_odometry\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0)
+imageView_odometry\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0)
+imageView_odometry\confidence_shown=false
+imageView_odometry\depth_shown=false
+imageView_odometry\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+imageView_odometry\features_shown=true
+imageView_odometry\features_size=0
+imageView_odometry\graphics_view=false
+imageView_odometry\graphics_view_scale=true
+imageView_odometry\graphics_view_scale_to_height=false
+imageView_odometry\image_shown=true
+imageView_odometry\lines_shown=true
+imageView_odometry\lines_width=0
+imageView_odometry\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_odometry\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+imageView_source\alpha=100
+imageView_source\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_source\colormap=2
+imageView_source\colormap_camera_frame=true
+imageView_source\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0)
+imageView_source\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0)
+imageView_source\confidence_shown=false
+imageView_source\depth_shown=false
+imageView_source\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+imageView_source\features_shown=true
+imageView_source\features_size=0
+imageView_source\graphics_view=false
+imageView_source\graphics_view_scale=true
+imageView_source\graphics_view_scale_to_height=false
+imageView_source\image_shown=true
+imageView_source\lines_shown=true
+imageView_source\lines_width=0
+imageView_source\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_source\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+multisession_imageview\alpha=100
+multisession_imageview\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+multisession_imageview\colormap=2
+multisession_imageview\colormap_camera_frame=true
+multisession_imageview\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0)
+multisession_imageview\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0)
+multisession_imageview\confidence_shown=false
+multisession_imageview\depth_shown=false
+multisession_imageview\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+multisession_imageview\features_shown=true
+multisession_imageview\features_size=0
+multisession_imageview\graphics_view=false
+multisession_imageview\graphics_view_scale=true
+multisession_imageview\graphics_view_scale_to_height=false
+multisession_imageview\image_shown=true
+multisession_imageview\lines_shown=true
+multisession_imageview\lines_width=0
+multisession_imageview\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+multisession_imageview\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+widget_cloudViewer\camera_axis_shown=true
+widget_cloudViewer\camera_focal=@Variant(\0\0\0T8\xb6\0\0\x37s\0\0\xb5\xd5\0\0)
+widget_cloudViewer\camera_free=false
+widget_cloudViewer\camera_lockZ=true
+widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0\xc0\x9b\x36@\x80\xf7\xea\x41\x17p\xe)
+widget_cloudViewer\camera_target_follow=true
+widget_cloudViewer\camera_target_locked=false
+widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0?\x80\0\0)
+widget_cloudViewer\color_range_inverted=0
+widget_cloudViewer\color_range_max=0
+widget_cloudViewer\color_range_min=0
+widget_cloudViewer\coordinate_frame_scale=1
+widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0)
+widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0)
+widget_cloudViewer\frustum_shown=true
+widget_cloudViewer\grid=false
+widget_cloudViewer\grid_cell_count=50
+widget_cloudViewer\grid_cell_size=1
+widget_cloudViewer\intensity_max=100
+widget_cloudViewer\intensity_rainbow_colormap=false
+widget_cloudViewer\intensity_red_colormap=true
+widget_cloudViewer\normals=false
+widget_cloudViewer\normals_scale=0.20000000298023224
+widget_cloudViewer\normals_step=1
+widget_cloudViewer\rendering_rate=5
+widget_cloudViewer\trajectory_shown=true
+widget_cloudViewer\trajectory_size=100

rtabmap_examples/config/multi_rgbd_inertial_dataset_front.yaml

@@ -0,0 +1,27 @@
+#%YAML:1.0
+---
+camera_name: MULTI_RGBD_INERTIAL_DATASET_FRONT
+image_width: 640
+image_height: 360
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 319.1610, 0., 329.5844, 0.,
+       319.0881, 182.1654, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 4
+   data: [ -0.0535, 0.0589, -0.0176, 0.0 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 
+       0., 1., 0.,
+       0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 319.1610, 0., 329.5844, 0., 0.,
+       319.0881, 182.1654, 0., 0., 0., 1.,
+       0. ]

rtabmap_examples/config/multi_rgbd_inertial_dataset_left.yaml

@@ -0,0 +1,27 @@
+#%YAML:1.0
+---
+camera_name: MULTI_RGBD_INERTIAL_DATASET_LEFT
+image_width: 640
+image_height: 360
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 319.1506, 0., 321.8367, 0.,
+       318.9474, 180.0959, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 4
+   data: [ -0.0556, 0.0576, -0.0174, 0.0 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 
+       0., 1., 0.,
+       0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 319.1506, 0., 321.8367, 0., 0.,
+       318.9474, 180.0959, 0., 0., 0., 1.,
+       0. ]

rtabmap_examples/config/multi_rgbd_inertial_dataset_rear.yaml

@@ -0,0 +1,27 @@
+#%YAML:1.0
+---
+camera_name: MULTI_RGBD_INERTIAL_DATASET_REAR
+image_width: 640
+image_height: 360
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 319.3360, 0., 323.9265, 0.,
+       319.1947, 181.9133, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 4
+   data: [ -0.0534, 0.0542, -0.0155, 0.0 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 
+       0., 1., 0.,
+       0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 319.3360, 0., 323.9265, 0., 0.,
+       319.1947, 181.9133, 0., 0., 0., 1.,
+       0. ]

rtabmap_examples/config/multi_rgbd_inertial_dataset_right.yaml

@@ -0,0 +1,27 @@
+#%YAML:1.0
+---
+camera_name: MULTI_RGBD_INERTIAL_DATASET_RIGHT
+image_width: 640
+image_height: 360
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 320.7208, 0., 325.4179, 0.,
+       320.7359, 184.8089, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 4
+   data: [ -0.0531, 0.0549, -0.0166, 0.0 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 
+       0., 1., 0.,
+       0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 320.7208, 0., 325.4179, 0., 0.,
+       320.7359, 184.8089, 0., 0., 0., 1.,
+       0. ]

rtabmap_examples/launch/depthai.launch.py

@@ -1,8 +1,14 @@
 # Requirements:
 #   A OAK-D camera
-#   Install depthai-ros package (https://github.com/luxonis/depthai-ros)
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch!
 # Example:
 #   $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
+#
+# Description: In this example, we feed IR-D images to rtabmap
+#
+# Note: The first frames may be too bright or too dark till camera exposure adjusts 
+#       to an appriopriate level. Do "Detection->Reset odometry", then 
+#       "Edit->Delete memory" if tracking is lost on start.
 
 import os
 

rtabmap_examples/launch/depthai_color.launch.py

@@ -0,0 +1,81 @@
+# Requirements:
+#   A OAK-D camera
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch!
+# Example:
+#   $ ros2 launch rtabmap_examples depthai_color.launch.py camera_model:=OAK-D
+#
+# Description: In this example, we feed RGB-D images to rtabmap
+#
+# Note: The first frames may be too bright or too dark till camera exposure adjusts 
+#       to an appriopriate level. Do "Detection->Reset odometry", then 
+#       "Edit->Delete memory" if tracking is lost on start.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    parameters=[{'frame_id':'oak-d-base-frame',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False}]
+    
+    sync_parameters=[{'approx_sync':True,
+                      'approx_sync_max_interval':0.005}]
+    
+    remappings=[('imu', '/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('depthai_examples'), 'launch'),
+                '/stereo_inertial_node.launch.py']),
+                launch_arguments={'enableRviz': 'false',
+                                  'rgbResolution': '1080p',
+                                  'rgbScaleNumerator': '2', # Convert to 720p (same size than depth)
+                                  'rgbScaleDinominator': '3'}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=sync_parameters,
+            remappings=[('rgb/image', '/color/image'),
+                        ('rgb/camera_info', '/color/camera_info'),
+                        ('depth/image', '/stereo/depth')]),
+
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/imu')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        # VSLAM
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=parameters,
+            remappings=remappings)
+    ])

rtabmap_examples/launch/depthai_stereo.launch.py

@@ -0,0 +1,90 @@
+# Requirements:
+#   A OAK-D camera
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch!
+#
+#   Issue: To have compatible stereo camera info with rtabmap (P[0,3] should be positive on left camera info or negative in right camera info), 
+#          we should add the following line here (https://github.com/luxonis/depthai-ros/blob/887248d72cc6b9515793f828645346408b9cad47/depthai_examples/src/stereo_inertial_publisher.cpp#L593)
+#          so that left camera info has a positive P[0,3] instead of negative to correctly compute the baseline:
+#
+#   <line 593> leftCameraInfo.p[3] *=-1;
+# 
+# Example:
+#   $ ros2 launch rtabmap_examples depthai_stereo.launch.py camera_model:=OAK-D
+#
+# Description: In this example, we feed stereo IR images to rtabmap
+#
+# Note: The first frames may be too bright or too dark till camera exposure adjusts 
+#       to an appriopriate level. Do "Detection->Reset odometry", then 
+#       "Edit->Delete memory" if tracking is lost on start.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    parameters={'frame_id':'oak-d-base-frame',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}
+    
+    sync_parameters=[{'approx_sync':True,
+                      'approx_sync_max_interval':0.005}]
+
+    remappings=[('imu', '/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('depthai_examples'), 'launch'),
+                '/stereo_inertial_node.launch.py']),
+                launch_arguments={'depth_aligned': 'false', # not color mode
+                                  'enableRviz': 'false',
+                                  'monoResolution': '400p'}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='stereo_sync', output='screen',
+            parameters=sync_parameters,
+            remappings=[('left/image', '/left/image_rect'),
+                        ('left/camera_info', '/left/camera_info'),
+                        ('right/image', '/right/image_rect'),
+                        ('right/camera_info', '/right/camera_info')]),
+
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/imu')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='stereo_odometry', output='screen',
+            parameters=[parameters],
+            remappings=remappings),
+
+        # VSLAM
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters],
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=[parameters,
+                        {'odometry_node_name': "stereo_odometry"}],
+            remappings=remappings)
+    ])

rtabmap_examples/launch/euroc_datasets.launch.py

@@ -62,7 +62,8 @@ def generate_launch_description():
         # Nodes to launch
         Node(
             package='rtabmap_odom', executable='stereo_odometry', output='screen',
-            parameters=[parameters],
+            parameters=[parameters,
+                { 'always_process_most_recent_frame':True}],
             remappings=remappings),
 
         Node(
@@ -82,7 +83,8 @@ def generate_launch_description():
 
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=[parameters],
+            parameters=[parameters,
+                        {'odometry_node_name': "stereo_odometry"}],
             remappings=remappings),
         
         # Image rectification and publishing synchronized camera_info
@@ -103,15 +105,13 @@ def generate_launch_description():
             namespace='stereo_camera'),
             
         Node(
-            package='image_proc', executable='image_proc', output='screen',
+            package='image_proc', executable='rectify_node', output='screen',
             remappings=[
-              ('image_raw', '/cam0/image_raw'),
               ('image', '/cam0/image_raw')],
             namespace='stereo_camera/left'),
         Node(
-            package='image_proc', executable='image_proc', output='screen',
+            package='image_proc', executable='rectify_node', output='screen',
             remappings=[
-              ('image_raw', '/cam1/image_raw'),
               ('image', '/cam1/image_raw')],
             namespace='stereo_camera/right'),
 

rtabmap_examples/launch/lidar3d.launch.py

@@ -154,7 +154,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
   
     Node(
       package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-      parameters=[shared_parameters, rtabmap_parameters],
+      parameters=[shared_parameters, rtabmap_parameters,
+                  {'odometry_node_name': "icp_odometry"}],
       remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
   ]
   

rtabmap_examples/launch/lidar3d_assemble.launch.py

@@ -173,7 +173,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     # Just for visualization
     Node(
       package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-      parameters=[shared_parameters, rtabmap_parameters],
+      parameters=[shared_parameters, rtabmap_parameters,
+                  {'odometry_node_name': "icp_odometry"}],
       remappings=remappings + [('scan_cloud', viz_topic)])
   ]
   

rtabmap_examples/launch/lidar3d_assemble_x2.launch.py

@@ -201,7 +201,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
     # Just for visualization
     Node(
       package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-      parameters=[shared_parameters, rtabmap_parameters],
+      parameters=[shared_parameters, rtabmap_parameters,
+                  {'odometry_node_name': "icp_odometry"}],
       remappings=remappings + [('scan_cloud', viz_topic)])
   ]
   

rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch.py

@@ -0,0 +1,162 @@
+#
+#  Example launch file to run VSLAM on this dataset: https://github.com/seungsang07/multi-rgbd-inertial-dataset
+#
+#  Requirement(s):
+#    * RTAB-Map should be built with OpenGV support.
+#
+#  To convert ROS1 bags to ROS2 (https://docs.openvins.com/dev-ros1-to-ros2.html):
+#     sudo pip install rosbags
+#     rosbags-convert --src Indoor.bag --dst Indoor
+#
+#  Usage:
+#    ros2 launch rtabmap_examples multi_rgbd_inertial_dataset.launch.py
+#    ros2 bag play Indoor/Indoor.db3 --clock
+#
+#  Note(s):
+#    * Communication performance could be improved using the composable nodes of these nodes instead,
+#      but we are using nodes here to better understand what is going on with rqt_graph.
+#
+#  To get RMSE after the run (using https://github.com/MichaelGrupp/evo):
+#    rtabmap-export --poses --pose_format 10 ~/.ros/rtabmap.db
+#    evo_ape tum -a -p --plot_mode xy ~/Downloads/GroundTruth/gt_indoor.txt ~/.ros/rtabmap_poses.txt
+#
+
+from launch import LaunchDescription, LaunchContext
+from launch_ros.actions import SetParameter
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
+def make_yaml_to_camera_info_node(camera):
+  # the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them
+  return Node(
+        package='rtabmap_util', executable='yaml_to_camera_info.py', name=f'yaml_to_camera_info_{camera}', output='screen',
+        parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), f'/config/multi_rgbd_inertial_dataset_{camera}.yaml']}],
+        remappings=[
+          ('image', 'color/image_raw'),
+          ('camera_info', 'color/camera_info')],
+        namespace=f'camera_{camera}')
+
+def make_rgbd_sync_node(camera):
+  # synchronize topics of each camera together
+  return Node(
+        package='rtabmap_sync', executable='rgbd_sync', name=f'rgbd_sync_{camera}', output="screen",
+        parameters=[{"approx_sync": False}],
+        remappings=[
+            ("rgb/image", 'color/image_raw'),
+            ("depth/image", 'aligned_depth_to_color/image_raw'),
+            ("rgb/camera_info", 'color/camera_info'),
+            ("rgbd_image", 'rgbd_image')],
+        namespace=f'camera_{camera}')
+
+def make_static_transform_publisher_node(x, y, z, yaw, pitch, roll, parent_frame, child_frame):
+  return Node(
+          package='tf2_ros', executable='static_transform_publisher', name=f"static_transform_publisher_{parent_frame}_{child_frame}", output='screen',
+          arguments=['--frame-id', parent_frame, 
+                     '--child-frame-id', child_frame,
+                     '--x', str(x),
+                     '--y', str(y),
+                     '--z', str(z),
+                     '--yaw', str(yaw),
+                     '--pitch', str(pitch),
+                     '--roll', str(roll)])
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
+  
+  use_imu = LaunchConfiguration('use_imu').perform(context)
+  use_imu = use_imu == 'true' or use_imu == 'True'
+  
+  # Synchronize all cameras together
+  rgbdx_sync_node = Node(
+      package='rtabmap_sync', executable='rgbdx_sync', output="screen",
+      parameters=[{
+          "rgbd_cameras": 4,
+          "approx_sync": True,
+          "approx_sync_max_interval": 0.015}],
+      remappings=[
+          ("rgbd_image0", '/camera_left/rgbd_image'),
+          ("rgbd_image1", '/camera_front/rgbd_image'),
+          ("rgbd_image2", '/camera_right/rgbd_image'),
+          ("rgbd_image3", '/camera_rear/rgbd_image')],
+      namespace='rtabmap'
+  )
+  
+  # RGB-D odometry
+  remappings = []
+  if use_imu:
+    remappings = [("imu", '/imu')]
+  rgbd_odometry_node = Node(
+      package='rtabmap_odom', executable='rgbd_odometry', output="screen",
+      parameters=[{
+          "frame_id": 'base_link',
+          "rgbd_cameras": 0, # make it subscribe to rgbd_images topic from rgbdx_sync
+          "subscribe_rgbd": True,
+          "wait_imu_to_init": use_imu}],
+      remappings=remappings,
+      namespace='rtabmap'
+  )
+
+  # SLAM 
+  remappings=[("sensor_data", 'odom_sensor_data/raw')]
+  if(use_imu):
+    remappings.append(('imu', '/imu'))
+  slam_node = Node(
+      package='rtabmap_slam', executable='rtabmap', output="screen",
+      parameters=[{
+          "subscribe_sensor_data": True,
+          "frame_id": 'base_link',
+          "approx_sync": False,
+          "Grid/3D": 'false',
+          "Grid/RayTracing": 'true',
+          "Grid/NormalsSegmentation": 'false',
+          "Grid/MaxGroundHeight": '0.05',
+          "Rtabmap/CreateIntermediateNodes": 'true' # Only to record all odometry poses for trajectory evaluation purpose
+      }],
+      remappings=remappings,
+      arguments=["--delete_db_on_start"],
+      namespace='rtabmap'
+  )
+      
+  # Visualization
+  viz_node = Node(
+    package='rtabmap_viz', executable='rtabmap_viz', output="screen",
+    parameters=[{
+        "subscribe_sensor_data": True,
+        "frame_id": 'base_link',
+        "approx_sync": False,
+        "subscribe_odom_info": True
+    }],
+    remappings=[("sensor_data", 'odom_sensor_data/raw')],
+    arguments=["-d", [FindPackageShare('rtabmap_examples'), '/config/multi_rgbd_inertial_dataset.ini']],
+    namespace='rtabmap'
+  )
+  
+  return [
+        make_yaml_to_camera_info_node('left'),
+        make_rgbd_sync_node('left'),
+        make_yaml_to_camera_info_node('front'),
+        make_rgbd_sync_node('front'),
+        make_yaml_to_camera_info_node('right'),
+        make_rgbd_sync_node('right'),
+        make_yaml_to_camera_info_node('rear'),
+        make_rgbd_sync_node('rear'),
+        # The dataset doesn't provide /tf or /tf_static for the extrinsics between imu and the cameras, so we add them here
+        make_static_transform_publisher_node(0., 0., 0.22, 3.1415926, 0., 0., 'base_link', 'imu_link'),
+        make_static_transform_publisher_node(-0.099307, -0.208806, 0.024309, 3.108592, -0.051480, -1.592415, 'imu_link', 'camera_left_color_optical_frame'),
+        make_static_transform_publisher_node(-0.435392, 0.022256, 0.053441, 1.532258, -0.007768, -1.580204, 'imu_link', 'camera_front_color_optical_frame'),
+        make_static_transform_publisher_node(-0.063987, 0.212966, 0.032071, -0.010030, 0.021977, -1.553814, 'imu_link', 'camera_right_color_optical_frame'),
+        make_static_transform_publisher_node(0.178893, -0.006307, 0.017677, -1.606156, 0.027545, -1.587179, 'imu_link', 'camera_rear_color_optical_frame'),
+        make_static_transform_publisher_node(0.045872, -0.026775, 0.284806, 3.141063, -0.014869, -0.014969, 'imu_link', 'os_sensor'),
+        rgbdx_sync_node,
+        rgbd_odometry_node,
+        slam_node,
+        viz_node
+      ]
+
+def generate_launch_description():
+  return LaunchDescription([
+    DeclareLaunchArgument('use_imu', default_value='false', description='Use IMU'),
+    SetParameter(name='use_sim_time', value=True),
+    OpaqueFunction(function=launch_setup),
+  ])

rtabmap_examples/launch/realsense_d435i_color.launch.py

@@ -37,6 +37,15 @@ def generate_launch_description():
 
         # Make sure IR emitter is enabled
         SetParameter(name='depth_module.emitter_enabled', value=1),
+        
+        DeclareLaunchArgument(
+            'args', default_value='',
+            description='Extra arguments set to rtabmap and odometry nodes.'),
+        
+        DeclareLaunchArgument(
+            'odom_args', default_value='',
+            description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
+
 
         # Launch camera driver
         IncludeLaunchDescription(
@@ -55,13 +64,14 @@ def generate_launch_description():
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
             parameters=parameters,
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
             remappings=remappings),
 
         Node(
             package='rtabmap_slam', executable='rtabmap', output='screen',
             parameters=parameters,
             remappings=remappings,
-            arguments=['-d']),
+            arguments=['-d', LaunchConfiguration("args")]),
 
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',

rtabmap_examples/launch/realsense_d435i_color_composition.launch.py

@@ -0,0 +1,130 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch rtabmap_examples realsense_d435i_color_composition.launch.py
+#
+# This is the "composition" variant of realsense_d435i_color.launch.py: the
+# camera driver, IMU filter, RGB-D odometry and SLAM all run as composable
+# nodes in a single component container (rtabmap_container) with
+# use_intra_process_comms enabled, so messages can be passed by pointer instead
+# of being serialized/copied between processes.
+#
+# As in the non-composed example, the color stream is used as RGB and paired
+# with the depth aligned to color (align_depth.enable), with the IR emitter on.
+#
+# Notes:
+#  * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py:
+#    that launch file always starts the camera as a standalone node and exposes
+#    no way to load it into an existing container. Instead we instantiate the
+#    camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the
+#    same way realsense's own rs_intra_process_demo_launch.py does.
+#  * ComposableNode has no "arguments" field, so the args/odom_args/-d
+#    command-line mechanism of the non-composed example is not available here.
+#    To override rtabmap parameters, add them directly to the 'parameters' dict
+#    below. '-d' (delete database on start) becomes the 'delete_db_on_start'
+#    parameter.
+#
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    parameters={
+          'frame_id':'camera_link',
+          'subscribe_depth':True,
+          'subscribe_odom_info':True,
+          'approx_sync':False,
+          'wait_imu_to_init':True}
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('rgb/image', '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image', '/camera/aligned_depth_to_color/image_raw')]
+
+    # Enable zero-copy intra-process communication on every composable node.
+    intra_process = [{'use_intra_process_comms': True}]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'unite_imu_method', default_value='2',
+            description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
+        DeclareLaunchArgument(
+            'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
+
+        # Single component container holding the whole pipeline.
+        ComposableNodeContainer(
+            name='rtabmap_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            output='screen',
+            composable_node_descriptions=[
+
+                # Camera driver (replaces the rs_launch.py include).
+                ComposableNode(
+                    package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory',
+                    name='camera', namespace='',
+                    parameters=[{
+                        'enable_gyro': True,
+                        'enable_accel': True,
+                        'unite_imu_method': LaunchConfiguration('unite_imu_method'),
+                        'align_depth.enable': True,
+                        'enable_sync': True,
+                        'rgb_camera.profile': '640x360x30',
+                        'depth_module.emitter_enabled': 1}], # Make sure IR emitter is enabled
+                    extra_arguments=intra_process),
+
+                # Compute quaternion of the IMU
+                ComposableNode(
+                    package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos',
+                    name='imu_filter', namespace='',
+                    parameters=[{'use_mag': False,
+                                 'world_frame':'enu',
+                                 'publish_tf':False}],
+                    remappings=[('imu/data_raw', '/camera/imu')],
+                    extra_arguments=intra_process),
+
+                # RGB-D odometry (color + depth aligned to color)
+                ComposableNode(
+                    package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry',
+                    parameters=[parameters],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+
+                # SLAM
+                # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph
+                # topics by default (transient_local QoS), which is incompatible
+                # with intra-process comms ("intraprocess communication allowed
+                # only with volatile durability"). latch=False makes them volatile
+                # so the node can join the zero-copy container.
+                ComposableNode(
+                    package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper',
+                    parameters=[parameters,
+                      {'delete_db_on_start': True, # Equivalent of '-d'
+                       'latch': False}],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+            ]),
+
+        # Visualization:
+        # Note: rtabmap_viz is launched as a standalone node, not as a component.
+        # It is a Qt application and its UI must run in the process main thread,
+        # while components run in container worker threads, so it cannot be
+        # composed (the same applies to rviz2).
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            condition=IfCondition(LaunchConfiguration('rtabmap_viz')),
+            parameters=[parameters],
+            remappings=remappings),
+    ])

rtabmap_examples/launch/realsense_d435i_combined.launch.py

@@ -0,0 +1,106 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch rtabmap_examples realsense_d435i_combined.launch.py
+#
+# Description: In this example, we feed visual odometry with IR stereo images 
+#              for better pose estimation while seding RGB-D data to slam for 
+#              a colored map.
+#
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node, SetParameter
+from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    vo_parameters={
+          'frame_id':'camera_link',
+          'wait_imu_to_init':True}
+
+    vo_remappings=[
+          ('imu', '/imu/data'),
+          ('left/image_rect', '/camera/infra1/image_rect_raw'),
+          ('left/camera_info', '/camera/infra1/camera_info'),
+          ('right/image_rect', '/camera/infra2/image_rect_raw'),
+          ('right/camera_info', '/camera/infra2/camera_info')]
+    
+    slam_parameters={
+          'frame_id':'camera_link',
+          'subscribe_depth':True,
+          'subscribe_odom_info':True,
+          'approx_sync':False}
+
+    slam_remappings=[
+          ('imu', '/imu/data'),
+          ('rgb/image', '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image', '/camera/aligned_depth_to_color/image_raw')]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'unite_imu_method', default_value='2',
+            description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
+
+        #Hack to disable IR emitter
+        SetParameter(name='depth_module.emitter_enabled', value=0),
+        
+        DeclareLaunchArgument(
+            'args', default_value='',
+            description='Extra arguments set to rtabmap and odometry nodes.'),
+        
+        DeclareLaunchArgument(
+            'odom_args', default_value='',
+            description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
+
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('realsense2_camera'), 'launch'),
+                '/rs_launch.py']),
+                launch_arguments={'camera_namespace': '',
+                                  'enable_gyro': 'true',
+                                  'enable_accel': 'true',
+                                  'unite_imu_method': LaunchConfiguration('unite_imu_method'),
+                                  'enable_infra1': 'true',
+                                  'enable_infra2': 'true',
+                                  'align_depth.enable': 'true',
+                                  'enable_sync': 'true',
+                                  'rgb_camera.profile': '640x360x30'}.items(),
+        ),
+
+        Node(
+            package='rtabmap_odom', executable='stereo_odometry', output='screen',
+            parameters=[vo_parameters],
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
+            remappings=vo_remappings),
+
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[slam_parameters],
+            remappings=slam_remappings,
+            arguments=['-d', LaunchConfiguration("args")]),
+
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=[slam_parameters,
+                        {'odometry_node_name': "stereo_odometry"}],
+            remappings=slam_remappings),
+                
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/camera/imu')]),
+    ])

rtabmap_examples/launch/realsense_d435i_infra.launch.py

@@ -35,6 +35,14 @@ def generate_launch_description():
         DeclareLaunchArgument(
             'unite_imu_method', default_value='2',
             description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
+        
+        DeclareLaunchArgument(
+            'args', default_value='',
+            description='Extra arguments set to rtabmap and odometry nodes.'),
+        
+        DeclareLaunchArgument(
+            'odom_args', default_value='',
+            description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
 
         #Hack to disable IR emitter
         SetParameter(name='depth_module.emitter_enabled', value=0),
@@ -56,13 +64,14 @@ def generate_launch_description():
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
             parameters=parameters,
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
             remappings=remappings),
 
         Node(
             package='rtabmap_slam', executable='rtabmap', output='screen',
             parameters=parameters,
             remappings=remappings,
-            arguments=['-d']),
+            arguments=['-d', LaunchConfiguration("args")]),
 
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',

rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py

@@ -0,0 +1,132 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch rtabmap_examples realsense_d435i_infra_composition.launch.py
+#
+# This is the "composition" variant of realsense_d435i_infra.launch.py: the
+# camera driver, IMU filter, RGB-D odometry and SLAM all run as composable
+# nodes in a single component container (rtabmap_container) with
+# use_intra_process_comms enabled, so messages can be passed by pointer instead
+# of being serialized/copied between processes.
+#
+# As in the non-composed example, the left infrared image (infra1) is used as
+# the grayscale "RGB" input and paired with the depth stream. This works because
+# on the D435i the depth is computed in the left-infrared frame, so infra1 and
+# depth share the same intrinsics/frame (already registered, no align needed).
+#
+# Notes:
+#  * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py:
+#    that launch file always starts the camera as a standalone node and exposes
+#    no way to load it into an existing container. Instead we instantiate the
+#    camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the
+#    same way realsense's own rs_intra_process_demo_launch.py does.
+#  * ComposableNode has no "arguments" field, so the args/odom_args/-d
+#    command-line mechanism of the non-composed example is not available here.
+#    To override rtabmap parameters, add them directly to the 'parameters' dict
+#    below. '-d' (delete database on start) becomes the 'delete_db_on_start'
+#    parameter.
+#
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    parameters={
+          'frame_id':'camera_link',
+          'subscribe_depth':True,
+          'subscribe_odom_info':True,
+          'approx_sync':False,
+          'wait_imu_to_init':True}
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('rgb/image', '/camera/infra1/image_rect_raw'),
+          ('rgb/camera_info', '/camera/infra1/camera_info'),
+          ('depth/image', '/camera/depth/image_rect_raw')]
+
+    # Enable zero-copy intra-process communication on every composable node.
+    intra_process = [{'use_intra_process_comms': True}]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'unite_imu_method', default_value='2',
+            description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
+        DeclareLaunchArgument(
+            'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
+
+        # Single component container holding the whole pipeline.
+        ComposableNodeContainer(
+            name='rtabmap_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            output='screen',
+            composable_node_descriptions=[
+
+                # Camera driver (replaces the rs_launch.py include).
+                ComposableNode(
+                    package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory',
+                    name='camera', namespace='',
+                    parameters=[{
+                        'enable_gyro': True,
+                        'enable_accel': True,
+                        'unite_imu_method': LaunchConfiguration('unite_imu_method'),
+                        'enable_infra1': True,
+                        'enable_infra2': True,
+                        'enable_sync': True,
+                        'depth_module.emitter_enabled': 0}], # Hack to disable IR emitter
+                    extra_arguments=intra_process),
+
+                # Compute quaternion of the IMU
+                ComposableNode(
+                    package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos',
+                    name='imu_filter', namespace='',
+                    parameters=[{'use_mag': False,
+                                 'world_frame':'enu',
+                                 'publish_tf':False}],
+                    remappings=[('imu/data_raw', '/camera/imu')],
+                    extra_arguments=intra_process),
+
+                # RGB-D odometry (infra1 as grayscale RGB + depth)
+                ComposableNode(
+                    package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry',
+                    parameters=[parameters],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+
+                # SLAM
+                # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph
+                # topics by default (transient_local QoS), which is incompatible
+                # with intra-process comms ("intraprocess communication allowed
+                # only with volatile durability"). latch=False makes them volatile
+                # so the node can join the zero-copy container.
+                ComposableNode(
+                    package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper',
+                    parameters=[parameters,
+                      {'delete_db_on_start': True, # Equivalent of '-d'
+                       'latch': False}],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+            ]),
+
+        # Visualization:
+        # Note: rtabmap_viz is launched as a standalone node, not as a component.
+        # It is a Qt application and its UI must run in the process main thread,
+        # while components run in container worker threads, so it cannot be
+        # composed (the same applies to rviz2).
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            condition=IfCondition(LaunchConfiguration('rtabmap_viz')),
+            parameters=[parameters],
+            remappings=remappings),
+    ])

rtabmap_examples/launch/realsense_d435i_stereo.launch.py

@@ -3,7 +3,21 @@
 #   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
 # Example:
 #   $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py
-
+#
+#
+#
+#
+# VINS-Fusion example:
+#   Add to your ros2 workspace the package https://github.com/zinuok/VINS-Fusion-ROS2
+#   Apply this patch https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd
+#      -> Revert "#define USE_GPU" change if you want to build VINS-Fusion with GPU support.
+#   That may be counterintuitive, but we need to build VINS-Fusion first, then rebuild rtabmap with VINS-Fusion support.
+#      -> in your ros2 workspace, do "colcon build --packages-select vins"
+#      -> go back under rtabmap library repo, then rebuild/install with "cmake -DWITH_VINS_FUSION=ON ..."
+#      -> do "colcon build" in your ros2 workspace again to rebuild rtabmap_ros
+#   $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py odom_args:="--Odom/Strategy 9 OdomVINSFusion/ConfigPath ~/ros2_ws/src/VINS-Fusion-ROS2/config/realsense_d435i/realsense_stereo_imu_config.yaml"
+#      -> set "imu: 1" in realsense_stereo_imu_config.yaml to do stereo inertial odometry, otherwise only stereo odometry is done.
+#
 import os
 
 from ament_index_python.packages import get_package_share_directory
@@ -16,11 +30,11 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 
 def generate_launch_description():
-    parameters=[{
+    parameters={
           'frame_id':'camera_link',
           'subscribe_stereo':True,
           'subscribe_odom_info':True,
-          'wait_imu_to_init':True}]
+          'wait_imu_to_init':True}
 
     remappings=[
           ('imu', '/imu/data'),
@@ -38,6 +52,15 @@ def generate_launch_description():
 
         #Hack to disable IR emitter
         SetParameter(name='depth_module.emitter_enabled', value=0),
+        
+        DeclareLaunchArgument(
+            'args', default_value='',
+            description='Extra arguments set to rtabmap and odometry nodes.'),
+        
+        DeclareLaunchArgument(
+            'odom_args', default_value='',
+            description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
+
 
         # Launch camera driver
         IncludeLaunchDescription(
@@ -55,18 +78,20 @@ def generate_launch_description():
 
         Node(
             package='rtabmap_odom', executable='stereo_odometry', output='screen',
-            parameters=parameters,
+            parameters=[parameters],
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
             remappings=remappings),
 
         Node(
             package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=parameters,
+            parameters=[parameters],
             remappings=remappings,
-            arguments=['-d']),
+            arguments=['-d', LaunchConfiguration("args")]),
 
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=parameters,
+            parameters=[parameters,
+                        {'odometry_node_name': "stereo_odometry"}],
             remappings=remappings),
                 
         # Compute quaternion of the IMU

rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py

@@ -0,0 +1,128 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch rtabmap_examples realsense_d435i_stereo_composition.launch.py
+#
+# This is the "composition" variant of realsense_d435i_stereo.launch.py: the
+# camera driver, IMU filter, stereo odometry and SLAM all run as composable
+# nodes in a single component container (rtabmap_container) with
+# use_intra_process_comms enabled, so messages can be passed by pointer instead
+# of being serialized/copied between processes.
+#
+# Notes:
+#  * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py:
+#    that launch file always starts the camera as a standalone node and exposes
+#    no way to load it into an existing container. Instead we instantiate the
+#    camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the
+#    same way realsense's own rs_intra_process_demo_launch.py does.
+#  * ComposableNode has no "arguments" field, so the args/odom_args/-d
+#    command-line mechanism of the non-composed example is not available here.
+#    To override rtabmap parameters, add them directly to the 'parameters' dict
+#    below. '-d' (delete database on start) becomes the 'delete_db_on_start'
+#    parameter.
+#
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    parameters={
+          'frame_id':'camera_link',
+          'subscribe_stereo':True,
+          'subscribe_odom_info':True,
+          'wait_imu_to_init':True}
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('left/image_rect', '/camera/infra1/image_rect_raw'),
+          ('left/camera_info', '/camera/infra1/camera_info'),
+          ('right/image_rect', '/camera/infra2/image_rect_raw'),
+          ('right/camera_info', '/camera/infra2/camera_info')]
+
+    # Enable zero-copy intra-process communication on every composable node.
+    intra_process = [{'use_intra_process_comms': True}]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'unite_imu_method', default_value='2',
+            description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
+        DeclareLaunchArgument(
+            'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
+
+        # Single component container holding the whole pipeline.
+        ComposableNodeContainer(
+            name='rtabmap_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            output='screen',
+            composable_node_descriptions=[
+
+                # Camera driver (replaces the rs_launch.py include).
+                ComposableNode(
+                    package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory',
+                    name='camera', namespace='',
+                    parameters=[{
+                        'enable_gyro': True,
+                        'enable_accel': True,
+                        'unite_imu_method': LaunchConfiguration('unite_imu_method'),
+                        'enable_infra1': True,
+                        'enable_infra2': True,
+                        'enable_sync': True,
+                        'depth_module.emitter_enabled': 0}], # Hack to disable IR emitter
+                    extra_arguments=intra_process),
+
+                # Compute quaternion of the IMU
+                ComposableNode(
+                    package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos',
+                    name='imu_filter', namespace='',
+                    parameters=[{'use_mag': False,
+                                 'world_frame':'enu',
+                                 'publish_tf':False}],
+                    remappings=[('imu/data_raw', '/camera/imu')],
+                    extra_arguments=intra_process),
+
+                # Stereo odometry
+                ComposableNode(
+                    package='rtabmap_odom', plugin='rtabmap_odom::StereoOdometry',
+                    parameters=[parameters],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+
+                # SLAM
+                # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph
+                # topics by default (transient_local QoS), which is incompatible
+                # with intra-process comms ("intraprocess communication allowed
+                # only with volatile durability"). latch=False makes them volatile
+                # so the node can join the zero-copy container.
+                ComposableNode(
+                    package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper',
+                    parameters=[parameters,
+                      {'delete_db_on_start': True, # Equivalent of '-d'
+                       'latch': False}],
+                    remappings=remappings,
+                    extra_arguments=intra_process),
+            ]),
+
+        # Visualization:
+        # Note: rtabmap_viz is launched as a standalone node, not as a component.
+        # It is a Qt application and its UI must run in the process main thread,
+        # while components run in container worker threads, so it cannot be
+        # composed (the same applies to rviz2).
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            condition=IfCondition(LaunchConfiguration('rtabmap_viz')),
+            parameters=[parameters,
+                        {'odometry_node_name': "stereo_odometry"}],
+            remappings=remappings),
+    ])

rtabmap_examples/launch/rtabmap_D405x2.launch.py

@@ -74,7 +74,6 @@ def generate_launch_description():
         remappings=[
             ("rgbd_image", '/realsense_camera1/rgbd_image'),
             ("odom", 'odom')],
-        arguments=["--delete_db_on_start", ''],
         prefix='',
         namespace='rtabmap'
     )

rtabmap_examples/launch/rtabmap_D405x3.launch.py

@@ -88,7 +88,6 @@ def generate_launch_description():
         remappings=[
             ("rgbd_image", '/realsense_camera1/rgbd_image'),
             ("odom", 'odom')],
-        arguments=["--delete_db_on_start", ''],
         prefix='',
         namespace='rtabmap'
     )

rtabmap_examples/launch/zed.launch.py

@@ -23,13 +23,26 @@ remappings = []
 
 def launch_setup(context: LaunchContext, *args, **kwargs):
 
-    # Hack to override grab_resolution parameter without changing any files
+    use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]
+
+    # Override some ZED parameters without changing any files:
+    #  * grab_resolution: VGA
+    #  * pos_tracking_enabled: disabled when rtabmap computes the odometry, so
+    #    the ZED node does not publish the odom->camera_link TF (which would
+    #    conflict with rtabmap's odometry). We still set publish_tf:=true below
+    #    so the ZED node keeps broadcasting the IMU TF, which rtabmap needs
+    #    (wait_imu_to_init). sensors.publish_imu_tf is ignored when publish_tf
+    #    is false, and the IMU frame is not in the ZED URDF, so this is the only
+    #    way to get the IMU TF while rtabmap owns the odometry.
+    pos_tracking_enabled = 'true' if use_zed_odometry else 'false'
     with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
         zed_override_file.write("---\n"+
                   "/**:\n"+
                   "    ros__parameters:\n"+
                   "        general:\n"+
-                  "            grab_resolution: 'VGA'")
+                  "            grab_resolution: 'VGA'\n"+
+                  "        pos_tracking:\n"+
+                  "            pos_tracking_enabled: "+pos_tracking_enabled)
 
     parameters=[{'frame_id':'zed_camera_link',
                  'subscribe_rgbd':True,
@@ -38,7 +51,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
 
     remappings=[('imu', '/zed/zed_node/imu/data')]
 
-    if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]:
+    if use_zed_odometry:
         remappings.append(('odom', '/zed/zed_node/odom'))
     else:
         parameters.append({'subscribe_odom_info': True})
@@ -51,7 +64,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
                 '/zed_camera.launch.py']),
                 launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
                                     'ros_params_override_path': zed_override_file.name,
-                                    'publish_tf': LaunchConfiguration('use_zed_odometry'),
+                                    'publish_tf': 'true',
+                                    'publish_imu_tf': 'true',
                                     'publish_map_tf': 'false'}.items(),
         ),
 
@@ -59,8 +73,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
         Node(   
             package='rtabmap_sync', executable='rgbd_sync', output='screen',
             parameters=parameters,
-            remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
-                        ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
+            remappings=[('rgb/image', '/zed/zed_node/rgb/color/rect/image'),
+                        ('rgb/camera_info', '/zed/zed_node/rgb/color/rect/camera_info'),
                         ('depth/image', '/zed/zed_node/depth/depth_registered')]),
 
         # Visual odometry

rtabmap_examples/launch/zed_composition.launch.py

@@ -0,0 +1,164 @@
+# Requirements:
+#   A ZED camera
+#   Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
+# Example:
+#   $ ros2 launch rtabmap_examples zed_composition.launch.py camera_model:=zed2i
+#
+# This is the "composition" variant of zed.launch.py: the ZED driver, RGB-D
+# synchronization, visual odometry and SLAM all run as composable nodes in a
+# single component container with use_intra_process_comms enabled, so messages
+# can be passed by pointer instead of being serialized/copied between processes.
+#
+# Notes:
+#  * The ZED wrapper's zed_camera.launch.py already creates its own component
+#    container ("zed_container") and loads the ZedCamera component into it with
+#    intra-process comms enabled by default (enable_ipc:=true). So instead of
+#    creating our own container, we let the ZED wrapper create it and load the
+#    rtabmap nodes into the SAME container (/zed/zed_container) with
+#    LoadComposableNodes. This assumes the default ZED namespace ("zed"), which
+#    is independent of camera_model.
+#  * ComposableNode has no "arguments" or "condition" field. So the '-d'
+#    argument becomes the 'delete_db_on_start' parameter, and the conditional
+#    odometry node is included in Python depending on use_zed_odometry.
+#
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription, LaunchContext
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node, LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import tempfile
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
+
+    use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]
+
+    # Override some ZED parameters without changing any files:
+    #  * grab_resolution: VGA
+    #  * pos_tracking_enabled: disabled when rtabmap computes the odometry, so
+    #    the ZED node does not publish the odom->camera_link TF (which would
+    #    conflict with rtabmap's odometry). We still set publish_tf:=true below
+    #    so the ZED node keeps broadcasting the IMU TF, which rtabmap needs
+    #    (wait_imu_to_init). sensors.publish_imu_tf is ignored when publish_tf
+    #    is false, and the IMU frame is not in the ZED URDF, so this is the only
+    #    way to get the IMU TF while rtabmap owns the odometry.
+    pos_tracking_enabled = 'true' if use_zed_odometry else 'false'
+    with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
+        zed_override_file.write("---\n"+
+                  "/**:\n"+
+                  "    ros__parameters:\n"+
+                  "        general:\n"+
+                  "            grab_resolution: 'VGA'\n"+
+                  "        pos_tracking:\n"+
+                  "            pos_tracking_enabled: "+pos_tracking_enabled)
+
+    # ZED topics are /zed/zed_node/* and the container created by the wrapper is
+    # /zed/zed_container (default ZED namespace "zed").
+    zed_ns = '/zed/zed_node'
+    zed_container = '/zed/zed_container'
+
+    parameters=[{'frame_id':'zed_camera_link',
+                 'subscribe_rgbd':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}]
+
+    remappings=[('imu', zed_ns + '/imu/data')]
+
+    if use_zed_odometry:
+        remappings.append(('odom', zed_ns + '/odom'))
+    else:
+        parameters.append({'subscribe_odom_info': True})
+
+    # Enable zero-copy intra-process communication on every composable node.
+    intra_process = [{'use_intra_process_comms': True}]
+
+    # rtabmap nodes loaded into the ZED container.
+    composable_nodes = [
+        # Sync rgb/depth/camera_info together
+        ComposableNode(
+            package='rtabmap_sync', plugin='rtabmap_sync::RGBDSync',
+            parameters=parameters,
+            remappings=[('rgb/image', zed_ns + '/rgb/color/rect/image'),
+                        ('rgb/camera_info', zed_ns + '/rgb/color/rect/camera_info'),
+                        ('depth/image', zed_ns + '/depth/depth_registered')],
+            extra_arguments=intra_process),
+    ]
+
+    # Visual odometry (only when not using ZED's own odometry). ComposableNode
+    # has no 'condition', so we add it here based on use_zed_odometry.
+    if not use_zed_odometry:
+        composable_nodes.append(
+            ComposableNode(
+                package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry',
+                parameters=parameters,
+                remappings=remappings,
+                extra_arguments=intra_process))
+
+    # VSLAM
+    # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph topics
+    # by default (transient_local QoS), which is incompatible with intra-process
+    # comms ("intraprocess communication allowed only with volatile durability").
+    # latch=False makes them volatile so the node can join the container.
+    composable_nodes.append(
+        ComposableNode(
+            package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper',
+            parameters=parameters + [{'delete_db_on_start': True, # Equivalent of '-d'
+                                      'latch': False}],
+            remappings=remappings,
+            extra_arguments=intra_process))
+
+    return [
+        # Launch camera driver. It creates the "zed_container" component
+        # container (enable_ipc:=true by default) and loads the ZedCamera
+        # component into it.
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('zed_wrapper'), 'launch'),
+                '/zed_camera.launch.py']),
+                launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
+                                    'ros_params_override_path': zed_override_file.name,
+                                    # publish_tf must be true so the ZED node broadcasts the
+                                    # IMU TF (gated by publish_tf). The odom->camera_link TF is
+                                    # disabled via pos_tracking_enabled=false (override file)
+                                    # when rtabmap computes the odometry.
+                                    'publish_tf': 'true',
+                                    'publish_imu_tf': 'true',
+                                    'publish_map_tf': 'false'}.items(),
+        ),
+
+        # Load the rtabmap pipeline into the ZED container.
+        LoadComposableNodes(
+            target_container=zed_container,
+            composable_node_descriptions=composable_nodes),
+
+        # Visualization
+        # Note: rtabmap_viz is a Qt application; its UI must run in the process
+        # main thread, while components run in container worker threads. So it
+        # cannot be composed and stays a standalone node.
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=parameters,
+            remappings=remappings)
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_zed_odometry', default_value='false',
+            description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
+
+        DeclareLaunchArgument(
+            'camera_model', default_value='',
+            description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_examples/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_examples</name>
-  <version>0.22.1</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's example launch files.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_launch/launch/rtabmap.launch.py

@@ -40,7 +40,17 @@ class ConditionalBool(Substitution):
             return self.text_else
             
 def launch_setup(context, *args, **kwargs):      
-               
+
+    rtabmap_viz_odometry_node_name = "rgbd_odometry"
+    use_icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
+    use_icp_odometry = use_icp_odometry == 'true' or use_icp_odometry == 'True'
+    use_stereo_odometry = LaunchConfiguration('stereo').perform(context)
+    use_stereo_odometry = use_stereo_odometry == 'true' or use_stereo_odometry == 'True'
+    if use_icp_odometry:
+        rtabmap_viz_odometry_node_name = "icp_odometry"
+    elif use_stereo_odometry:
+        rtabmap_viz_odometry_node_name = "stereo_odometry"
+
     return [
         DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''),
         DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''),
@@ -53,6 +63,7 @@ def launch_setup(context, *args, **kwargs):
         DeclareLaunchArgument('qos_user_data',   default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'),
         DeclareLaunchArgument('qos_imu',         default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'),
         DeclareLaunchArgument('qos_gps',         default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'),
+        DeclareLaunchArgument('qos_env_sensor',         default_value=LaunchConfiguration('qos'), description='Specific QoS used for env sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
         
         DeclareLaunchArgument('odom_log_level',  default_value=LaunchConfiguration('log_level'), description='Specific ROS logger level for odometry node.'),
         
@@ -186,7 +197,8 @@ def launch_setup(context, *args, **kwargs):
                 "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
                 "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
                 "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
-                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'),
+                "always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}],
             remappings=[
                 ("rgb/image", LaunchConfiguration('rgb_topic_relay')),
                 ("depth/image", LaunchConfiguration('depth_topic_relay')),
@@ -223,7 +235,8 @@ def launch_setup(context, *args, **kwargs):
                 "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
                 "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
                 "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
-                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'),
+                "always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}],
             remappings=[
                 ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
                 ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
@@ -258,7 +271,8 @@ def launch_setup(context, *args, **kwargs):
                 "qos_imu": LaunchConfiguration('qos_imu'),
                 "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
                 "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
-                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation'),
+                "always_process_most_recent_frame": LaunchConfiguration('odom_always_process_most_recent_frame')}],
             remappings=[
                 ("scan", LaunchConfiguration('scan_topic')),
                 ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
@@ -303,6 +317,7 @@ def launch_setup(context, *args, **kwargs):
                 "qos_camera_info": LaunchConfiguration('qos_camera_info'),
                 "qos_imu": LaunchConfiguration('qos_imu'),
                 "qos_gps": LaunchConfiguration('qos_gps'),
+                "qos_env_sensor": LaunchConfiguration('qos_env_sensor'),
                 "qos_user_data": LaunchConfiguration('qos_user_data'),
                 "scan_normal_k": LaunchConfiguration('scan_normal_k'),
                 "landmark_linear_variance": LaunchConfiguration('tag_linear_variance'),
@@ -327,6 +342,7 @@ def launch_setup(context, *args, **kwargs):
                 ("gps/fix", LaunchConfiguration('gps_topic')),
                 ("tag_detections", LaunchConfiguration('tag_topic')),
                 ("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
+                ("env_sensor", LaunchConfiguration('env_sensor_topic')),
                 ("odom", LaunchConfiguration('odom_topic')),
                 ("imu", LaunchConfiguration('imu_topic')),
                 ("goal_out", LaunchConfiguration('output_goal_topic'))],
@@ -356,7 +372,8 @@ def launch_setup(context, *args, **kwargs):
                 "qos_scan": LaunchConfiguration('qos_scan'),
                 "qos_odom": LaunchConfiguration('qos_odom'),
                 "qos_camera_info": LaunchConfiguration('qos_camera_info'),
-                "qos_user_data": LaunchConfiguration('qos_user_data')
+                "qos_user_data": LaunchConfiguration('qos_user_data'),
+                "odometry_node_name": rtabmap_viz_odometry_node_name
             }],
             remappings=[
                 ("rgb/image", LaunchConfiguration('rgb_topic_relay')),
@@ -495,6 +512,7 @@ def generate_launch_description():
         DeclareLaunchArgument('odom_guess_frame_id',        default_value='',      description=''),
         DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0',   description=''),
         DeclareLaunchArgument('odom_guess_min_rotation',    default_value='0.0',   description=''),
+        DeclareLaunchArgument('odom_always_process_most_recent_frame', default_value='true', description='Odometry: always process latest frame to reduce delay, skipping frames in case odometry is slower than camera frame rate. In case you want to make sure to process all frames (e.g., from a rosbag/dataset) and you don\'t care about delay, set this to false.'),
 
         # imu
         DeclareLaunchArgument('imu_topic',        default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'),
@@ -514,6 +532,8 @@ def generate_launch_description():
         DeclareLaunchArgument('tag_linear_variance',  default_value='0.0001',          description=''),
         DeclareLaunchArgument('tag_angular_variance', default_value='9999.0',            description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
         DeclareLaunchArgument('fiducial_topic',       default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'),
+        
+        DeclareLaunchArgument('env_sensor_topic',     default_value='/env_sensor', description='A rtabmap_msgs/EnvSensor topic.'),
         OpaqueFunction(function=launch_setup)
     ])
 

rtabmap_launch/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_launch</name>
-  <version>0.22.1</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's main launch files.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_msgs/msg/EnvSensor.msg

@@ -1,6 +1,27 @@
 
 std_msgs/Header header
 
-# EnvSensor
+# Environmental sensor
+
+# built-in types
+int32 TYPE_UNDEFINED=0
+int32 TYPE_WIFI_SIGNAL_STRENGTH=1      # dBm
+int32 TYPE_AMBIENT_TEMPERATURE=2       # Celcius
+int32 TYPE_AMBIENT_AIR_PRESSURE=3      # hPa
+int32 TYPE_AMBIENT_LIGHT=4             # lx
+int32 TYPE_AMBIENT_RELATIVE_HUMIDITY=5 # %
+
+# user types
+int32 TYPE_CUSTOM1=100
+int32 TYPE_CUSTOM2=101
+int32 TYPE_CUSTOM3=102
+int32 TYPE_CUSTOM4=103
+int32 TYPE_CUSTOM5=104
+int32 TYPE_CUSTOM6=105
+int32 TYPE_CUSTOM7=106
+int32 TYPE_CUSTOM8=107
+int32 TYPE_CUSTOM9=108
+
 int32 type
+
 float64 value

rtabmap_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_msgs</name>
-  <version>0.22.1</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's msgs package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_odom/CMakeLists.txt

@@ -41,7 +41,25 @@ include_directories(
 )
 
 SET(Libraries
+   cv_bridge::cv_bridge
+   rclcpp_components::component
+   image_geometry::image_geometry
+   laser_geometry::laser_geometry
+   message_filters::message_filters
+   pcl_conversions::pcl_conversions
+)
+SET(PublicLibraries
+   rclcpp::rclcpp
+   sensor_msgs::sensor_msgs
+   nav_msgs::nav_msgs
+   rtabmap_conversions::rtabmap_conversions
+   rtabmap_msgs::rtabmap_msgs
+   rtabmap_util::rtabmap_util
+   rtabmap_sync::rtabmap_sync
+)
+SET(AmentLibraries
    cv_bridge
+   rclcpp_components
    image_geometry
    laser_geometry
    message_filters
@@ -56,6 +74,10 @@ SET(Libraries
    rtabmap_sync
 )
 
+IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+  add_definitions(-DPRE_ROS_LYRICAL)
+ENDIF()
+
 ###########
 ## Build ##
 ###########
@@ -81,10 +103,13 @@ target_include_directories(rtabmap_odom
 )
 add_library(rtabmap_odom_plugins SHARED ${rtabmap_odom_plugins_lib_src})
 
-ament_target_dependencies(rtabmap_odom ${Libraries})
-ament_target_dependencies(rtabmap_odom_plugins ${Libraries})
-
-target_link_libraries(rtabmap_odom_plugins rtabmap_odom)
+if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+   ament_target_dependencies(rtabmap_odom ${AmentLibraries})
+else()
+   target_link_libraries(rtabmap_odom PRIVATE ${Libraries} PUBLIC ${PublicLibraries})
+   target_link_libraries(rtabmap_odom_plugins PUBLIC ${Libraries})
+endif()
+target_link_libraries(rtabmap_odom_plugins PUBLIC rtabmap_odom)
 
 rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::RGBDOdometry")
 rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::StereoOdometry")
@@ -92,25 +117,21 @@ rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::ICPOdometry
 
 
 add_executable(rtabmap_rgbd_odometry src/RGBDOdometryNode.cpp)
-ament_target_dependencies(rtabmap_rgbd_odometry ${Libraries})
-target_link_libraries(rtabmap_rgbd_odometry rtabmap_odom_plugins)
+target_link_libraries(rtabmap_rgbd_odometry PRIVATE rtabmap_odom_plugins)
 set_target_properties(rtabmap_rgbd_odometry PROPERTIES OUTPUT_NAME "rgbd_odometry")
 
 add_executable(rtabmap_stereo_odometry src/StereoOdometryNode.cpp)
-ament_target_dependencies(rtabmap_stereo_odometry ${Libraries})
-target_link_libraries(rtabmap_stereo_odometry rtabmap_odom_plugins)
+target_link_libraries(rtabmap_stereo_odometry PRIVATE rtabmap_odom_plugins)
 set_target_properties(rtabmap_stereo_odometry PROPERTIES OUTPUT_NAME "stereo_odometry")
 
 add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp)
-ament_target_dependencies(rtabmap_icp_odometry ${Libraries})
-target_link_libraries(rtabmap_icp_odometry rtabmap_odom_plugins)
+target_link_libraries(rtabmap_icp_odometry PRIVATE rtabmap_odom_plugins)
 set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry")
 
 #############
 ## Install ##
 #############
-
-ament_export_dependencies(${Libraries})
+ament_export_dependencies(${AmentLibraries})
 ament_export_include_directories(include)
 ament_export_targets(${PROJECT_NAME})          # To include downstream with targets
 ament_export_libraries(rtabmap_odom rtabmap_odom_plugins) # To include downstream without targets

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -30,9 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rclcpp/rclcpp.hpp"
 
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.hpp>
+#include <tf2_ros/buffer.hpp>
+#include <tf2_ros/transform_listener.hpp>
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 
@@ -98,7 +98,7 @@ protected:
 	virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
 
 private:
-
+	void processData();
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 	virtual void updateParameters(rtabmap::ParametersMap &) {}
@@ -123,6 +123,8 @@ private:
 	double guessMinTranslation_;
 	double guessMinRotation_;
 	double guessMinTime_;
+	double guessLinearVariance_;
+	double guessAngularVariance_;
 	bool publishTf_;
 	double waitForTransform_;
 	bool publishNullWhenLost_;
@@ -174,9 +176,12 @@ private:
 	rtabmap::Transform guessPreviousPose_;
 	double previousStamp_;
 	double previousClockTime_;
+	double lastReceivedTopicClock_;
+	double lastReceivedTopicStamp_;
 	double expectedUpdateRate_;
 	double maxUpdateRate_;
 	double minUpdateRate_;
+	bool alwaysProcessMostRecentFrame_;
 	std::string compressionImgFormat_;
 	bool compressionParallelized_;
 	int odomStrategy_;

rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp

@@ -78,6 +78,7 @@ private:
 	double scanNormalGroundUp_;
 	bool deskewing_;
 	bool deskewingSlerp_;
+	int topicQueueSize_;
 	//std::vector<std::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
 	//pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
 	bool scanReceived_ = false;

rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp

@@ -151,6 +151,7 @@ private:
 	int topicQueueSize_;
 	int syncQueueSize_;
 	bool keepColor_;
+	double approxSyncMaxInterval_;
 };
 
 }

rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp

@@ -149,6 +149,7 @@ private:
 	int topicQueueSize_;
 	int syncQueueSize_;
 	bool keepColor_;
+	double approxSyncMaxInterval_;
 };
 
 }

rtabmap_odom/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_odom</name>
-  <version>0.22.1</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's odometry package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_odom/src/OdometryROS.cpp

@@ -76,6 +76,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	guessMinTranslation_(0.0),
 	guessMinRotation_(0.0),
 	guessMinTime_(0.0),
+	guessLinearVariance_(0.001),
+	guessAngularVariance_(0.001),
 	publishTf_(true),
 	waitForTransform_(0.1), // 100 ms
 	publishNullWhenLost_(true),
@@ -89,9 +91,12 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	icpParams_(false),
 	previousStamp_(0.0),
 	previousClockTime_(0.0),
+	lastReceivedTopicClock_(0.0),
+	lastReceivedTopicStamp_(0.0),
 	expectedUpdateRate_(0.0),
 	maxUpdateRate_(0.0),
 	minUpdateRate_(0.0),
+	alwaysProcessMostRecentFrame_(true),
 	compressionImgFormat_(".jpg"),
 	compressionParallelized_(true),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
@@ -140,10 +145,13 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	guessMinTranslation_ = this->declare_parameter("guess_min_translation", guessMinTranslation_);
 	guessMinRotation_ = this->declare_parameter("guess_min_rotation", guessMinRotation_);
 	guessMinTime_ = this->declare_parameter("guess_min_time", guessMinTime_);
+	guessLinearVariance_ = this->declare_parameter("guess_linear_variance", guessLinearVariance_);
+	guessAngularVariance_ = this->declare_parameter("guess_angular_variance", guessAngularVariance_);
 
 	expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_);
 	maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_);
 	minUpdateRate_ = this->declare_parameter("min_update_rate", minUpdateRate_);
+	alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_);
 
 	compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_);
 	compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_);
@@ -201,6 +209,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_translation  = %f", guessMinTranslation_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_rotation     = %f", guessMinRotation_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time         = %f", guessMinTime_);
+	RCLCPP_INFO(this->get_logger(), "Odometry: guess_linear_variance  = %f", guessLinearVariance_);
+	RCLCPP_INFO(this->get_logger(), "Odometry: guess_angular_variance = %f", guessAngularVariance_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate   = %f Hz", expectedUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate        = %f Hz", maxUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate        = %f Hz", minUpdateRate_);
@@ -360,15 +370,16 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
 		odometry_->reset(initialPose_);
 	}
 
-	resetSrv_ = this->create_service<std_srvs::srv::Empty>("reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-	resetToPoseSrv_ = this->create_service<rtabmap_msgs::srv::ResetPose>("reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-	pauseSrv_ = this->create_service<std_srvs::srv::Empty>("pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-	resumeSrv_ = this->create_service<std_srvs::srv::Empty>("resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	const std::string servicePrefix = get_name() + std::string("/");
+	resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	resetToPoseSrv_ = this->create_service<rtabmap_msgs::srv::ResetPose>(servicePrefix + "reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
-	setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>("log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-	setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>("log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-	setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>("log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-	setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>("log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
 	odomStrategy_ = 0;
 	Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
@@ -461,25 +472,47 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header)
 {
 	//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
+	double clockNow = rtabmap_conversions::timestampFromROS(now());
 	if(dataMutex_.lockTry() == 0)
 	{
 		if(bufferedDataToProcess_) {
-			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!",
+			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.",
 						rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp));
 			++droppedMsgs_;
 		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
 		bufferedDataToProcess_ = false;
-		dataReady_.release();
+		if(alwaysProcessMostRecentFrame_) {
+			dataReady_.release();
+		}
 		dataMutex_.unlock();
 		++processedMsgs_;
+		if(!alwaysProcessMostRecentFrame_) {
+			processData();
+		}
 	}
 	else
 	{
-		//RCLCPP_WARN(get_logger(), "Dropping image/scan data");
+		double estimatedPeriod = clockNow - lastReceivedTopicClock_;
+		double topicPeriod = rtabmap_conversions::timestampFromROS(header.stamp) - lastReceivedTopicStamp_;
+		if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.5) {
+			RCLCPP_WARN(get_logger(), 
+			"Dropping image/scan data with stamp %f (delay=%f). Something is wrong "
+			"because the clock difference with the previous topic received (%fs) is much lower than the "
+			"expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing "
+			"a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" "
+			"to avoid aggressively dropping data.",
+			rtabmap_conversions::timestampFromROS(header.stamp),
+			clockNow - rtabmap_conversions::timestampFromROS(header.stamp),
+			estimatedPeriod,
+			topicPeriod,
+			lastReceivedTopicStamp_);
+		}
 		++droppedMsgs_;
 	}
+	lastReceivedTopicStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
+	lastReceivedTopicClock_ = clockNow;
 }
 
 void OdometryROS::mainLoopKill()
@@ -497,7 +530,10 @@ void OdometryROS::mainLoop()
 		// thread killed
 		return;
 	}
-
+	processData();
+}
+void OdometryROS::processData()
+{
 	UScopeMutex lock(dataMutex_);
 
 	// aliases
@@ -516,21 +552,35 @@ void OdometryROS::mainLoop()
 
 		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
 		{
-			RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.",
-					data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+			if(imus_.empty()) {
+				// If empty, it is an error!
+				RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.",
+						data.stamp());
+			}
 			bufferedDataToProcess_ = true;
 			return;
 		}
 		// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
 		std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
+		std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterLast = iterEnd;
 		if(iterEnd!= imus_.end())
 		{
 			++iterEnd;
 		}
-		for(std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iter=imus_.begin(); iter!=iterEnd;)
+		std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterFirst = imus_.begin();
+		for(std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iter=iterFirst; iter!=iterEnd;)
 		{
-			imus.push_back(*iter);
-			imus_.erase(iter++);
+			// Because we always keep the last processed imu in the buffer, skip the first 
+			// one when processing again the buffer
+			if(iter!=iterFirst) {
+				imus.push_back(*iter);
+			}
+			if(iter!=iterLast) {
+				imus_.erase(iter++);
+			}
+			else {
+				++iter;
+			}
 		}
 	} // end imu lock
 
@@ -666,6 +716,44 @@ void OdometryROS::mainLoop()
 		}
 	}
 
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
+	if(tooOldPreviousData)
+	{
+		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
+				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
+				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
+
+		if(!guess_.isNull())
+		{
+			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
+					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
+			odometry_->reset(odometry_->getPose() * guess_);
+			guess_.setNull();
+			guessPreviousPose_.setNull();
+		}
+		else
+		{
+			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
+			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
+			if(tfPose.isNull())
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
+						odomFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(tfPose);
+			}
+		}
+	}
+	
+	bool skipOdometryUpdate = false;
+
+	rtabmap::Transform pose;
+	rtabmap::OdometryInfo info;
+	rtabmap::Transform guessVelocity;
 
 	Transform guessCurrentPose;
 	if(!guessFrameId_.empty())
@@ -702,28 +790,22 @@ void OdometryROS::mainLoop()
 				   (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ < guessMinTime_)))
 				{
 					// Ignore odometry update, we didn't move enough
-					if(publishTf_)
-					{
-						geometry_msgs::msg::TransformStamped correctionMsg;
-						correctionMsg.child_frame_id = guessFrameId_;
-						correctionMsg.header.frame_id = odomFrameId_;
-						correctionMsg.header.stamp = header.stamp;
-						Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
-						rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
+					pose = odometry_->getPose() * guess_;
+					info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1);
+					info.reg.covariance.at<double>(0,0) = guessLinearVariance_;  // xx
+					info.reg.covariance.at<double>(1,1) = guessLinearVariance_;  // yy
+					info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
+					info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
+					info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
+					info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw
 
-						double time_now = now().seconds();
-						if(time_now >= previousClockTime_) {
-							tfBroadcaster_->sendTransform(correctionMsg);
-						}
-						else {
-							RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.",
-								correctionMsg.header.frame_id.c_str(),
-								correctionMsg.child_frame_id.c_str(),
-								previousClockTime_ - time_now);
-						}
-					}
-					guessPreviousPose_ = guessCurrentPose;
-					return;
+					//set velocity
+					double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
+					UASSERT(dt>0.0);
+					// use part of guess matching dt
+					(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+					guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
+					skipOdometryUpdate = true;
 				}
 			}
 			guessPreviousPose_ = guessCurrentPose;
@@ -735,23 +817,21 @@ void OdometryROS::mainLoop()
 		}
 	}
 
-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_;
-
 	// process data
 	rclcpp::Time timeStart = rclcpp::Clock().now();
-	rtabmap::OdometryInfo info;
 	if(!groundTruth.isNull())
 	{
 		data.setGroundTruth(groundTruth);
 	}
-	rtabmap::Transform pose;
-	if(!tooOldPreviousData)
+	if(!skipOdometryUpdate)
 	{
 		pose = odometry_->process(data, guess_, &info);
 	}
 	if(!pose.isNull())
 	{
-		guess_.setNull();
+		if(!skipOdometryUpdate) {
+			guess_.setNull();
+		}
 		resetCurrentCount_ = resetCountdown_;
 
 		//*********************
@@ -825,11 +905,16 @@ void OdometryROS::mainLoop()
 			odom.pose.covariance.at(35) = info.reg.covariance.at<double>(5,5)*2; // yawyaw
 
 			//set velocity
-			bool setTwist = !odometry_->getVelocityGuess().isNull();
+			bool setTwist = !guessVelocity.isNull() || !odometry_->getVelocityGuess().isNull();
 			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
-				odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				if(skipOdometryUpdate) {
+					UASSERT(!guessVelocity.isNull());
+					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				} else {
+					odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				}
 				odom.twist.twist.linear.x = x;
 				odom.twist.twist.linear.y = y;
 				odom.twist.twist.linear.z = z;
@@ -874,7 +959,7 @@ void OdometryROS::mainLoop()
 			odomLocalMap_->publish(cloudMsg);
 		}
 
-		if(odomLastFrame_->get_subscription_count())
+		if(!skipOdometryUpdate && odomLastFrame_->get_subscription_count())
 		{
 			// check which type of Odometry is using
 			if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
@@ -1005,20 +1090,14 @@ void OdometryROS::mainLoop()
 
 	}
 
-	if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData))
+	if(pose.isNull() && resetCurrentCount_ > 0)
 	{
-		if(tooOldPreviousData)
-		{
-			RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
-					"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
-					rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
-		}
-		else if(--resetCurrentCount_>0)
+		if(--resetCurrentCount_>0)
 		{
 			RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
 		}
 
-		if(resetCurrentCount_ == 0 || tooOldPreviousData)
+		if(resetCurrentCount_ == 0)
 		{
 			if(!guess_.isNull())
 			{
@@ -1181,9 +1260,11 @@ void OdometryROS::mainLoop()
 		msg.header.stamp = header.stamp; // use corresponding time stamp to image
 		odomSensorDataCompressedPub_->publish(msg);
 	}
-
 	double delay =  (now()-header.stamp).seconds(); 
-	if(visParams_)
+	if(skipOdometryUpdate) {
+		RCLCPP_INFO(this->get_logger(), "Odom: <skipped: guess not moving enough>, std dev=%fm|%frad, update time=%fs, delay=%fs", pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
+	}
+	else if(visParams_)
 	{
 		if(icpParams_)
 		{
@@ -1241,6 +1322,8 @@ void OdometryROS::reset(const Transform & pose)
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	previousClockTime_ = 0.0;
+	lastReceivedTopicClock_ = 0.0;
+	lastReceivedTopicStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -60,6 +60,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 	scanNormalGroundUp_(0.0),
 	deskewing_(false),
 	deskewingSlerp_(false),
+	topicQueueSize_(1),
 	scanReceived_(false),
 	cloudReceived_(false)
 {
@@ -83,6 +84,7 @@ void ICPOdometry::onOdomInit()
 	scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_);
 	deskewing_ = this->declare_parameter("deskewing", deskewing_);
 	deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_);
+	topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
 
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos                    = %d", (int)qos());
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
@@ -96,12 +98,13 @@ void ICPOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing              = %s", deskewing_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp        = %s", deskewingSlerp_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: topic_queue_size       = %d", topicQueueSize_);
 
 	rclcpp::SubscriptionOptions options;
 	options.callback_group = dataCallbackGroup_;
 
-	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
-	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options);
+	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
+	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options);
 
 	filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
 

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -65,7 +65,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 		exactSync6_(0),
 		topicQueueSize_(10),
 		syncQueueSize_(5),
-		keepColor_(false)
+		keepColor_(false),
+		approxSyncMaxInterval_(0.0)
 {
 	OdometryROS::init(false, true, false);
 }
@@ -91,9 +92,8 @@ void RGBDOdometry::onOdomInit()
 	int rgbdCameras = 1;
 	bool approxSync = true;
 	bool subscribeRGBD = false;
-	double approxSyncMaxInterval = 0.0;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
-	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_);
 	topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
 	int queueSize = this->declare_parameter("queue_size", -1);
 	if(queueSize != -1)
@@ -113,12 +113,20 @@ void RGBDOdometry::onOdomInit()
 		rgbdCameras = 0;
 	}
 	keepColor_ = this->declare_parameter("keep_color", keepColor_);
-	std::string rgbdTransport = this->declare_parameter("rgb_transport", std::string("raw"));
+	std::string rgbTransport = this->declare_parameter("rgb_transport", std::string("raw"));
+	if(rgbTransport != "raw") {
+		RCLCPP_WARN(this->get_logger(), "Parameter \"rgb_transport\" has been renamed "
+				"to \"image_transport\" and will be removed "
+				"in future versions! The value (%s) is copied to "
+				"\"image_transport\".", rgbTransport.c_str());
+	}
+	std::string imageTransport = this->declare_parameter("image_transport", rgbTransport);
 	std::string depthTransport = this->declare_parameter("depth_transport", std::string("raw"));
+	
 
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
 	if(approxSync)
-		RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
+		RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: topic_queue_size = %d", topicQueueSize_);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size  = %d", syncQueueSize_);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos             = %d", (int)qos());
@@ -126,7 +134,7 @@ void RGBDOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");
-	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgb_transport   = %s", rgbdTransport.c_str());
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: image_transport = %s", imageTransport.c_str());
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: depth_transport = %s", depthTransport.c_str());
 
 	rclcpp::SubscriptionOptions options;
@@ -165,8 +173,8 @@ void RGBDOdometry::onOdomInit()
 							MyApproxSync2Policy(syncQueueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_);
-					if(approxSyncMaxInterval > 0.0)
-						approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					if(approxSyncMaxInterval_ > 0.0)
+						approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 					approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
 				}
 				else
@@ -180,7 +188,7 @@ void RGBDOdometry::onOdomInit()
 				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
-						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image2_sub_.getSubscriber()->get_topic_name());
 			}
@@ -193,8 +201,8 @@ void RGBDOdometry::onOdomInit()
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_);
-					if(approxSyncMaxInterval > 0.0)
-							approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					if(approxSyncMaxInterval_ > 0.0)
+							approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 					approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 				}
 				else
@@ -209,7 +217,7 @@ void RGBDOdometry::onOdomInit()
 				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
-						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image3_sub_.getSubscriber()->get_topic_name());
@@ -224,8 +232,8 @@ void RGBDOdometry::onOdomInit()
 							rgbd_image2_sub_,
 							rgbd_image3_sub_,
 							rgbd_image4_sub_);
-					if(approxSyncMaxInterval > 0.0)
-						approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					if(approxSyncMaxInterval_ > 0.0)
+						approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 					approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 				}
 				else
@@ -241,7 +249,7 @@ void RGBDOdometry::onOdomInit()
 				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
-						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image3_sub_.getSubscriber()->get_topic_name(),
@@ -258,8 +266,8 @@ void RGBDOdometry::onOdomInit()
 							rgbd_image3_sub_,
 							rgbd_image4_sub_,
 							rgbd_image5_sub_);
-					if(approxSyncMaxInterval > 0.0)
-						approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					if(approxSyncMaxInterval_ > 0.0)
+						approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 					approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
 				}
 				else
@@ -276,7 +284,7 @@ void RGBDOdometry::onOdomInit()
 				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n  %s \\\n  %s \\\n   %s \\\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
-						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image3_sub_.getSubscriber()->get_topic_name(),
@@ -295,8 +303,8 @@ void RGBDOdometry::onOdomInit()
 							rgbd_image4_sub_,
 							rgbd_image5_sub_,
 							rgbd_image6_sub_);
-					if(approxSyncMaxInterval > 0.0)
-						approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					if(approxSyncMaxInterval_ > 0.0)
+						approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 					approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
 				}
 				else
@@ -314,7 +322,7 @@ void RGBDOdometry::onOdomInit()
 				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n  %s \\\n  %s \\\n   %s \\\n   %s \\\n   %s",
 						get_name(),
 						approxSync?"approx":"exact",
-						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 						rgbd_image1_sub_.getTopic().c_str(),
 						rgbd_image2_sub_.getTopic().c_str(),
 						rgbd_image3_sub_.getTopic().c_str(),
@@ -354,25 +362,26 @@ void RGBDOdometry::onOdomInit()
 	}
 	else
 	{
-		image_transport::TransportHints rgb_hints(this, "raw", "rgb_transport");
+		std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
+		std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
+#ifdef PRE_ROS_LYRICAL
+		image_transport::TransportHints rgb_hints(this); // using "image_transport" parameter
 		image_transport::TransportHints depth_hints(this, "raw", "depth_transport");
-
-		std::string rgb_topic = get_node_base_interface()->resolve_topic_or_service_name(
-      		"rgb/image", false, false
-		);
-		std::string depth_topic = get_node_base_interface()->resolve_topic_or_service_name(
-      		"depth/image", false, false
-		);
-
-		image_mono_sub_.subscribe(this, rgb_topic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
-		image_depth_sub_.subscribe(this, depth_topic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
+		image_mono_sub_.subscribe(this, rgbTopic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
+		image_depth_sub_.subscribe(this, depthTopic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
+#else
+		image_transport::TransportHints rgb_hints(*this); // using "image_transport" parameter
+		image_transport::TransportHints depth_hints(*this, "raw", "depth_transport");
+		image_mono_sub_.subscribe(*this, rgbTopic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), options);
+		image_depth_sub_.subscribe(*this, depthTopic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), options);
+#endif
 		info_sub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options);
 
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
-			if(approxSyncMaxInterval > 0.0)
-				approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+			if(approxSyncMaxInterval_ > 0.0)
+				approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
 			approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		}
 		else
@@ -385,7 +394,7 @@ void RGBDOdometry::onOdomInit()
 		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n   %s,\n   %s,\n   %s",
 				get_name(),
 				approxSync?"approx":"exact",
-				approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
 				topicQueueSize_,
 				syncQueueSize_,
 				image_mono_sub_.getSubscriber().getTopic().c_str(),
@@ -430,8 +439,10 @@ void RGBDOdometry::commonCallback(
 {
 	UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
 	rclcpp::Time higherStamp;
+	UASSERT_MSG(rgbImages[0], "RGB image is null!");
 	int imageWidth = rgbImages[0]->image.cols;
 	int imageHeight = rgbImages[0]->image.rows;
+	UASSERT_MSG(depthImages[0], "Depth image is null!");
 	int depthWidth = depthImages[0]->image.cols;
 	int depthHeight = depthImages[0]->image.rows;
 
@@ -445,6 +456,8 @@ void RGBDOdometry::commonCallback(
 	std::vector<rtabmap::CameraModel> cameraModels;
 	for(unsigned int i=0; i<rgbImages.size(); ++i)
 	{
+		UASSERT_MSG(rgbImages[i], uFormat("RGB image is null for camera %d", i).c_str());
+		UASSERT_MSG(depthImages[i], uFormat("Depth image is null for camera %d", i).c_str());
 		if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
@@ -589,12 +602,17 @@ void RGBDOdometry::callback(
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
 		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
-		imageMsgs[0] = cv_bridge::toCvShare(image);
-		depthMsgs[0] = cv_bridge::toCvShare(depth);
+		try{
+			imageMsgs[0] = cv_bridge::toCvShare(image);
+			depthMsgs[0] = cv_bridge::toCvShare(depth);
+		}
+		catch(cv::Exception& e) {
+			UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
+		}
 		infoMsgs.push_back(*cameraInfo);
 
 		double stampDiff = fabs(rtabmap_conversions::timestampFromROS(image->header.stamp) - rtabmap_conversions::timestampFromROS(depth->header.stamp));
-		if(stampDiff > 0.020)
+		if(approxSyncMaxInterval_==0.0 && stampDiff > 0.020)
 		{
 			RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
 					"high (diff=%fs, rgb=%fs, depth=%fs). You may want "