merged ros2->jazzy-devel

2026-10-03 16:27:46 +08:00 · 2025-06-08 16:04:29 -07:00
parent 4a1c257133 945101697a
commit 6ebb8e3184
55 changed files with 1043 additions and 221 deletions
@@ -0,0 +1,17 @@
+
+FROM introlab3it/rtabmap:jammy
+
+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}
+
+RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
+    chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
+
+RUN echo "source /opt/ros/humble/setup.bash" >> /home/${USERNAME}/.bashrc
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
@@ -1,12 +1,27 @@
 {
-    "image": "introlab3it/rtabmap_ros:humble-latest",
+    "build": {
+      "dockerfile": "Dockerfile",
+      "pull": true
+    },
+    "remoteUser": "vscode",
    "customizations": {
        "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+          "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/humble/lib/python3/dist-packages"
+      ],
+      "terminal.integrated.defaultProfile.linux": "bash"
    },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
-    "workspaceFolder": "/ros2_ws",
-    "postAttachCommand": "echo 'Initialize colcon: source /opt/ros/humble/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'",
-    "runArgs": ["--privileged"]
+    "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"]
 }
@@ -0,0 +1,20 @@
+
+FROM introlab3it/rtabmap:noble
+
+# 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}
+
+RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
+    chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
+
+RUN echo "source /opt/ros/jazzy/setup.bash" >> /home/${USERNAME}/.bashrc
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
@@ -1,12 +1,27 @@
 {
-    "image": "introlab3it/rtabmap_ros:jazzy-latest",
+    "build": {
+      "dockerfile": "Dockerfile",
+      "pull": true
+    },
+    "remoteUser": "vscode",
    "customizations": {
        "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+          "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/jazzy/lib/python3/dist-packages"
+      ],
+      "terminal.integrated.defaultProfile.linux": "bash"
    },
-    "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
-    "workspaceFolder": "/ros2_ws",
-    "postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'",
-    "runArgs": ["--privileged"]
+    "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"]
 }
@@ -0,0 +1,20 @@
+
+FROM introlab3it/rtabmap:noble-kilted
+
+# 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}
+
+RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
+    chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
+
+RUN echo "source /opt/ros/kilted/setup.bash" >> /home/${USERNAME}/.bashrc
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
@@ -0,0 +1,27 @@
+{
+    "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": "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"]
+}
@@ -0,0 +1,17 @@
+
+FROM introlab3it/rtabmap:focal
+
+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}
+
+RUN mkdir -p /home/${USERNAME}/catkin_ws/src && \
+    chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/catkin_ws
+
+RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USERNAME}/.bashrc
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
@@ -0,0 +1,27 @@
+{
+    "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/noetic/lib/python3/dist-packages"
+      ],
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "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"]
+}
@@ -10,8 +10,9 @@ jobs:
    runs-on: ubuntu-latest
    
    strategy:
+      fail-fast: false
      matrix:
-        docker_tag: [humble, humble-latest, jazzy, jazzy-latest]
+        docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted-latest]
        include:
        - docker_tag: humble
          docker_path: 'humble'
@@ -32,34 +33,39 @@ jobs:
          docker_platforms: |
            linux/amd64
            linux/arm64
+        - docker_tag: kilted-latest
+          docker_path: 'kilted/latest'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
    
    steps:
      -
        name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
      -
        name: Set up QEMU
-        uses: docker/setup-qemu-action@v1
+        uses: docker/setup-qemu-action@v3
        with:
          platforms: all
      -
        name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v1
+        uses: docker/setup-buildx-action@v3
      -
        name: Login to DockerHub
-        uses: docker/login-action@v1
+        uses: docker/login-action@v3
        with:
          username: ${{ secrets.DOCKERHUB_USERNAME }}
          password: ${{ secrets.DOCKERHUB_TOKEN }}
      -
        name: Build and push
-        uses: docker/build-push-action@v2
+        uses: docker/build-push-action@v6
        with:
          context: .
          push: true
          platforms: ${{ matrix.docker_platforms }}
          file: ./docker/${{ matrix.docker_path }}/Dockerfile
          tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
-          cache-from: type=registry,ref=introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
+          no-cache: true
          cache-to: type=inline

@@ -32,7 +32,6 @@ jobs:
          docker_platforms: |
            linux/amd64
            linux/arm64
-            linux/arm/v7
    
    steps:
      -
@@ -10,8 +10,13 @@ env:
  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
  BUILD_TYPE: Release

-jobs:
+jobs: 
  build:
+    # Disabling because Ubuntu 20.04 doesn't exist anymore on CI:
+    #   This is a scheduled Ubuntu 20.04 retirement. Ubuntu 20.04 LTS 
+    #   runner will be removed on 2025-04-15. For more details, see https://github.com/actions/runner-images/issues/11101
+    if: false
+
    # The CMake configure and build commands are platform agnostic and should work equally
    # well on Windows or Mac.  You can convert this to a matrix build if you need
    # cross-platform coverage.
@@ -8,26 +8,18 @@ on:
    branches: [ jazzy-devel ]

 env:
-  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
  BUILD_TYPE: Release

 jobs:
  build:
-    # The CMake configure and build commands are platform agnostic and should work equally
-    # well on Windows or Mac.  You can convert this to a matrix build if you need
-    # cross-platform coverage.
-    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build ros2 ${{ matrix.ros_distro }} on ubuntu ${{ matrix.ubuntu_distro }}
+    name: Build ros2 ${{ matrix.ros_distro }}
    runs-on: ubuntu-latest
    strategy:
      matrix:
        ros_distro: [jazzy]
-        include:
-        - ros_distro: 'jazzy'
-          ubuntu_distro: 'noble'
      fail-fast: false
    container:
-      image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu_distro }}-ros-${{ matrix.ros_distro }}-desktop-latest
+      image: osrf/ros:${{ matrix.ros_distro }}-desktop-full
    steps:   
    - uses: actions/checkout@v4
    - uses: ros-tooling/[email protected]
@@ -36,7 +28,7 @@ jobs:
    - 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.3
+    - uses: ros-tooling/action-ros-ci@v0.4
      with:
        package-name: rtabmap_ros
        target-ros2-distro: ${{ matrix.ros_distro }}
@@ -1,7 +1,7 @@
 rtabmap_ros
 ===========

-RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). 
+RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Humble minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). 

 #### CI Latest

@@ -34,10 +34,6 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current
            <td>Humble</td>
            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
        </tr>
-        <tr>
-            <td>Iron</td>
-            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
-        </tr>
        <tr>
            <td>Jazzy</td>
            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
@@ -72,9 +68,12 @@ export RCUTILS_COLORIZED_OUTPUT=1
 ```

 ## Recommended DDS
-If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files:
+If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`):
 ```bash
 export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+# Cyclone prefers multicast by default, if your router got too much spammed, 
+# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489):
+export CYCLONEDDS_URI="<Disc><DefaultMulticastAddress>0.0.0.0</></>"
 ```

 # Installation 
@@ -2,8 +2,8 @@

 * Available images on [introlab3it/rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros/):
    ```
-    foxy, foxy-latest
    humble, humble-latest
+    jazzy, jazzy-latest
    ```
    * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS. 

@@ -1,6 +1,5 @@
 FROM osrf/ros:jazzy-desktop
 # install rtabmap packages
-ARG CACHE_DATE=2016-01-01
 RUN apt-get update && apt-get install -y \
    ros-jazzy-rtabmap \
    ros-jazzy-rtabmap-ros \
@@ -12,7 +12,7 @@ RUN source /ros_entrypoint.sh && \
    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" && \
+    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 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
    cd && \
@@ -0,0 +1,6 @@
+FROM osrf/ros:kilted-desktop
+# install rtabmap packages
+RUN apt-get update && apt-get install -y \
+    ros-kilted-rtabmap \
+    ros-kilted-rtabmap-ros \
+    && rm -rf /var/lib/apt/lists/
@@ -0,0 +1,19 @@
+FROM introlab3it/rtabmap:24.04
+
+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="-j1" && \
+    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" && \
+    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 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    cd && \
+    rm -rf ros2_ws
@@ -19,7 +19,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)

-find_package(RTABMap 0.21.5 REQUIRED)
+find_package(RTABMap 0.22.0 REQUIRED)

 include_directories(
  ${CMAKE_CURRENT_SOURCE_DIR}/include
@@ -323,6 +323,42 @@ inline int sizeOfPointField(int datatype)
  }
  return -1;
 }
+
+template <typename K, typename V>
+typename std::map<K, V>::const_iterator getClosestIterator(
+	const std::map<K, V> & buffer,
+	const K & key)
+{
+	UASSERT(!buffer.empty());
+	typename std::map<K, V>::const_iterator iterB = buffer.lower_bound(key);
+	typename std::map<K, V>::const_iterator iterA = iterB;
+	if(iterA != buffer.begin())
+	{
+		iterA = --iterA;
+	}
+	if(iterB == buffer.end())
+	{
+		iterB = --iterB;
+	}
+	if(iterA == iterB)
+	{
+		return iterA;
+	}
+	if(iterA->first > key)
+	{
+		return iterA;
+	}
+	else if(iterB->first < key)
+	{
+		return iterB;
+	}
+	else if(key - iterA->first < iterB->first - key)
+	{
+		return iterA;
+	}
+	return iterB;
+}
+
 }

 #endif /* MSGCONVERSION_H_ */
@@ -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.21.10</version>
+  <version>0.22.0</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="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -1620,6 +1620,8 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
 	stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
 	stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
 	stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f));
+	stats.insert(std::make_pair("Odometry/localBundleAvgInlierDistance/pix", info.localBundleAvgInlierDistance));
+	stats.insert(std::make_pair("Odometry/localBundleMaxKeyFramesForInlier/", info.localBundleMaxKeyFramesForInlier));
 	stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f));
 	stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval));
 	stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled));
@@ -1650,9 +1652,8 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
 	{
 		if(!info.transform.isNull())
 		{
-			rtabmap::Transform diff = info.transformGroundTruth.inverse()*info.transform;
-			stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
-			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
+			stats.insert(std::make_pair("Odometry/TG_error_lin/m", info.transformGroundTruth.getDistance(info.transform)));
+			stats.insert(std::make_pair("Odometry/TG_error_ang/deg", info.transformGroundTruth.getAngle(info.transform)*180.0/CV_PI));
 		}

 		info.transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
@@ -1696,6 +1697,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, b
 	info.localBundleOutliers = msg.local_bundle_outliers;
 	info.localBundleConstraints = msg.local_bundle_constraints;
 	info.localBundleTime = msg.local_bundle_time;
+	info.localBundleAvgInlierDistance = msg.local_bundle_avg_inlier_distance;
+	info.localBundleMaxKeyFramesForInlier = msg.local_bundle_max_key_frames_for_inlier;
 	info.keyFrameAdded = msg.key_frame_added;
 	info.timeEstimation = msg.time_estimation;
 	info.timeParticleFiltering =  msg.time_particle_filtering;
@@ -1776,6 +1779,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::msg::OdomIn
 	msg.local_bundle_outliers = info.localBundleOutliers;
 	msg.local_bundle_constraints = info.localBundleConstraints;
 	msg.local_bundle_time = info.localBundleTime;
+	msg.local_bundle_avg_inlier_distance = info.localBundleAvgInlierDistance;
+	msg.local_bundle_max_key_frames_for_inlier = info.localBundleMaxKeyFramesForInlier;
 	UASSERT(info.localBundleModels.size() == info.localBundlePoses.size());
 	for(std::map<int, std::vector<rtabmap::CameraModel> >::const_iterator iter=info.localBundleModels.begin();
 		iter!=info.localBundleModels.end();
@@ -3380,7 +3385,7 @@ bool deskew_impl(
 			}
 		}
 	}
-	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
+	UDEBUG("Lidar deskewing time=%fs (slerp=%s waitForTransform=%f)", processingTime.elapsed(), slerp?"true":"false", waitForTransform);
 	return true;
 }

@@ -15,21 +15,25 @@
 + [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam)

 ### Outdoor Stereo VSLAM
-[stereo_outdoor_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/stereo_outdoor_demo.launch.py)
+[stereo_outdoor_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/stereo_outdoor_demo.launch.py) ([Video](https://youtu.be/qpTS7kg9J3A))

 ![Peek 2024-11-29 10-52](https://github.com/user-attachments/assets/b6dd4a1c-5bd5-4cfa-936d-e8e707bbcb23)
+
 ### Indoor 2D LiDAR and RGB-D SLAM
-[robot_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/robot_mapping_demo.launch.py)
+[robot_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/robot_mapping_demo.launch.py) (Videos: [rtabmap_viz](https://youtu.be/c0qrEd5rR7M), [rviz](https://youtu.be/MQoSDpAsqps))

 ![Peek 2024-11-29 11-07](https://github.com/user-attachments/assets/b02beeea-28ed-4fde-932d-c89bef1a046d)
+
 ### Multi-Session Indoor 2D LiDAR and RGB-D SLAM
-[multisession_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/multisession_mapping_demo.launch.py)
+[multisession_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/multisession_mapping_demo.launch.py) ([Video](https://youtu.be/XrnyhaxPCro))

 ![Peek 2024-11-29 11-48](https://github.com/user-attachments/assets/b130e5ab-618f-4c8b-840f-f926b65ab53b)
+
 ### Find-Object with SLAM
-[find_object_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/find_object_demo.launch.py)
+[find_object_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/find_object_demo.launch.py) ([Video](https://youtu.be/o1GSQanY-Do))

 ![Peek 2024-11-29 12-01](https://github.com/user-attachments/assets/b3cc0c67-517a-4f69-b4cc-35d288e96165)
+
 ### Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM
 [turtlebot4_sim_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py)

@@ -8,6 +8,9 @@
 #           <width>320</width>
 #           <height>240</height>
 #        </image>
+#   - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743):
+#        - <render_engine>ogre2</render_engine>
+#        + <render_engine>ogre</render_engine>
 #
 # Example with gazebo:
 #   1) Launch simulator (husky, nav2 and rtabmap):
@@ -8,6 +8,9 @@
 #           <width>320</width>
 #           <height>240</height>
 #        </image>
+#   - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743):
+#        - <render_engine>ogre2</render_engine>
+#        + <render_engine>ogre</render_engine>
 #
 # Example with gazebo:
 #   1) Launch simulator (husky, nav2 and rtabmap):
@@ -40,6 +43,8 @@ ARGUMENTS = [
                          description='Ignition World'),
    DeclareLaunchArgument('robot_ns', default_value='a200_0000',
                          description='Robot namespace'),
+    DeclareLaunchArgument('use_camera', default_value='true',
+                          description='Use camera for global loop closure / re-localization.'),
 ]

 def generate_launch_description():
@@ -83,6 +88,7 @@ def generate_launch_description():
            ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
            ('localization', LaunchConfiguration('localization')),
            ('use_sim_time', 'true'),
+            ('use_camera', LaunchConfiguration('use_camera')),
            ('robot_ns', LaunchConfiguration('robot_ns'))
        ]
    )
@@ -34,6 +34,7 @@ def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    localization = LaunchConfiguration('localization')
    robot_ns = LaunchConfiguration('robot_ns')
+    use_camera = LaunchConfiguration('use_camera')

    icp_odom_parameters={
          'odom_frame_id':'icp_odom',
@@ -43,7 +44,9 @@ def generate_launch_description():
    }

    rtabmap_parameters={
-          'subscribe_rgbd':True,
+          'subscribe_rgb':False,
+          'subscribe_depth':False,
+          'subscribe_rgbd': use_camera,
          'subscribe_scan_cloud':True,
          'use_action_for_goal':True,
          'odom_sensor_sync': True,
@@ -88,7 +91,7 @@ def generate_launch_description():
        DeclareLaunchArgument(
            'use_sim_time', default_value='false', choices=['true', 'false'],
            description='Use simulation (Gazebo) clock if true'),
-        
+
        DeclareLaunchArgument(
            'localization', default_value='false', choices=['true', 'false'],
            description='Launch rtabmap in localization mode (a map should have been already created).'),
@@ -97,8 +100,13 @@ def generate_launch_description():
            'robot_ns', default_value='a200_0000',
            description='Robot namespace.'),

+        DeclareLaunchArgument(
+            'use_camera', default_value='true',
+            description='Use camera for global loop closure / re-localization.'),
+
        # Nodes to launch
        Node(
+            condition=IfCondition(use_camera),
            package='rtabmap_sync', executable='rgbd_sync', output='screen',
            namespace=robot_ns,
            parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
@@ -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.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's demo launch files.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -8,7 +8,7 @@
 #
 #   If an IMU is used, make sure TF between lidar/base frame and imu is
 #     already calibrated. In this example, we assume the imu topic has 
-#     already the orientation estimated, it not, you can use 
+#     already the orientation estimated, if not, you can use 
 #     imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
 #     and set imu_topic to output topic of the filter.
 #
@@ -32,7 +32,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  imu_used =  imu_topic.perform(context) != ''
  
  rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
-  rgbd_image_used =  rgbd_image_topic.perform(context) != ''
+  rgbd_images_topic = LaunchConfiguration('rgbd_images_topic')
+  rgbd_image_used =  rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != ''
+  rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1
  
  voxel_size = LaunchConfiguration('voxel_size')
  voxel_size_value = float(voxel_size.perform(context))
@@ -46,13 +48,19 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  localization = LaunchConfiguration('localization').perform(context)
  localization = localization == 'true' or localization == 'True'
  
+  deskewing = LaunchConfiguration('deskewing').perform(context)
+  deskewing = deskewing == 'true' or deskewing == 'True'
+  
+  deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
+  deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
+  
  fixed_frame_from_imu = False
  fixed_frame_id =  LaunchConfiguration('fixed_frame_id').perform(context)
  if not fixed_frame_id and imu_used:
    fixed_frame_from_imu = True
    fixed_frame_id = frame_id.perform(context) + "_stabilized"
  
-  if not fixed_frame_id:
+  if not fixed_frame_id or not deskewing:
    lidar_topic_deskewed = lidar_topic
  
  # Rule of thumb:
@@ -78,10 +86,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  }
  
  icp_odometry_parameters = {
-    'expected_update_rate': 15.0,
-    'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below
+    'expected_update_rate': LaunchConfiguration('expected_update_rate'),
+    'deskewing': not fixed_frame_id and deskewing, # If fixed_frame_id is set, we do deskewing externally below
    'odom_frame_id': 'icp_odom',
    'guess_frame_id': fixed_frame_id,
+    'deskewing_slerp': deskewing_slerp,
    # RTAB-Map's internal parameters are strings:
    'Odom/ScanKeyFrameThr': '0.4',
    'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
@@ -97,6 +106,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
    'subscribe_rgb': False,
    'subscribe_odom_info': True,
    'subscribe_scan_cloud': True,
+    'map_frame_id': 'new_map',
+    'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
    # RTAB-Map's internal parameters are strings:
    'RGBD/ProximityMaxGraphDepth': '0',
    'RGBD/ProximityPathMaxNeighbors': '1',
@@ -106,7 +117,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
    'Mem/NotLinkedNodesKept': 'false',
    'Mem/STMSize': '30',
    'Reg/Strategy': '1',
-    'Icp/CorrespondenceRatio': '0.2'
+    'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
  }
  
  arguments = []
@@ -122,7 +133,10 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  else:
    remappings.append(('imu', 'imu_not_used'))
  if rgbd_image_used:
-    remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+    if rgbd_cameras == 1:
+      remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+    else:
+      remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
  
  nodes = [
    Node(
@@ -132,7 +146,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
    
    Node(
      package='rtabmap_slam', executable='rtabmap', output='screen',
-      parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}],
+      parameters=[shared_parameters, rtabmap_parameters, 
+                  {'subscribe_rgbd': rgbd_image_used, 
+                   'rgbd_cameras': rgbd_cameras}],
      remappings=remappings + [('scan_cloud', lidar_topic_deskewed)],
      arguments=arguments), 
  
@@ -154,7 +170,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
          'wait_for_transform_duration': 0.001}],
        remappings=[('imu/data', imu_topic)]))

-  if fixed_frame_id:
+  if fixed_frame_id and deskewing:
    # Lidar deskewing
    nodes.append(
      Node(
@@ -162,7 +178,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
        parameters=[{
          'use_sim_time': use_sim_time,
          'fixed_frame_id': fixed_frame_id,
-          'wait_for_transform': 0.2}],
+          'wait_for_transform': 0.2,
+          'slerp': deskewing_slerp}],
        remappings=[
            ('input_cloud', lidar_topic)
        ])
@@ -206,9 +223,25 @@ def generate_launch_description():
      'rgbd_image_topic', default_value='',
      description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
    
+    DeclareLaunchArgument(
+      'rgbd_images_topic', default_value='',
+      description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
+    
+    DeclareLaunchArgument(
+      'expected_update_rate', default_value='15.0',
+      description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
+    
    DeclareLaunchArgument(
      'voxel_size', default_value='0.1',
      description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
+    
+    DeclareLaunchArgument(
+      'min_loop_closure_overlap', default_value='0.2',
+      description='Minimum scan overlap pourcentage to accept a loop closure.'),
+    
+    DeclareLaunchArgument(
+      'deskewing_slerp', default_value='true',
+      description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),

    DeclareLaunchArgument(
      'qos', default_value='1',
@@ -8,7 +8,7 @@
 #   
 #   Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated.
 #     In this example, we assume the imu topic has 
-#     already the orientation estimated, it not, you can launch 
+#     already the orientation estimated, if not, you can launch 
 #     imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
 #     and set imu_topic to output topic of the filter.
 #
@@ -28,16 +28,23 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  
  frame_id = LaunchConfiguration('frame_id')

+  external_odom_frame_id =  LaunchConfiguration('external_odom_frame_id').perform(context)
+
  fixed_frame_from_imu = False
  fixed_frame_id =  LaunchConfiguration('fixed_frame_id').perform(context)
  if not fixed_frame_id:
-    fixed_frame_from_imu = True
-    fixed_frame_id = frame_id.perform(context) + "_stabilized"
+    if external_odom_frame_id:
+      fixed_frame_id = external_odom_frame_id
+    else:
+      fixed_frame_from_imu = True
+      fixed_frame_id = frame_id.perform(context) + "_stabilized"
  
  imu_topic = LaunchConfiguration('imu_topic')
  
  rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
-  rgbd_image_used =  rgbd_image_topic.perform(context) != ''
+  rgbd_images_topic = LaunchConfiguration('rgbd_images_topic')
+  rgbd_image_used =  rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != ''
+  rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1
  
  lidar_topic = LaunchConfiguration('lidar_topic')
  lidar_topic_value = lidar_topic.perform(context)
@@ -51,6 +58,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  localization = LaunchConfiguration('localization').perform(context)
  localization = localization == 'true' or localization == 'True'
  
+  deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
+  deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
+  
  # Rule of thumb:
  max_correspondence_distance = voxel_size_value * 10.0

@@ -74,7 +84,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  }

  icp_odometry_parameters = {
-    'expected_update_rate': 15.0,
+    'expected_update_rate': LaunchConfiguration('expected_update_rate'),
    'wait_imu_to_init': True,
    'odom_frame_id': 'icp_odom',
    'guess_frame_id': fixed_frame_id,
@@ -89,8 +99,9 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
  rtabmap_parameters = {
    'subscribe_depth': False,
    'subscribe_rgb': False,
-    'subscribe_odom_info': True,
+    'subscribe_odom_info': not external_odom_frame_id,
    'subscribe_scan_cloud': True,
+    'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""),
    'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
    # RTAB-Map's internal parameters are strings:
    'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
@@ -102,13 +113,16 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
    'Mem/NotLinkedNodesKept': 'false',
    'Mem/STMSize': '30',
    'Reg/Strategy': '1',
-    'Icp/CorrespondenceRatio': '0.2'
+    'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
  }
  
  remappings = [('imu', imu_topic),
                ('odom', 'icp_odom')]
  if rgbd_image_used:
-    remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+    if rgbd_cameras == 1:
+      remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+    else:
+      remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
    
  arguments = []
  if localization:
@@ -116,6 +130,11 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
    rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
  else:
    arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+    
+  if external_odom_frame_id:
+    viz_topic = lidar_topic_deskewed
+  else:
+    viz_topic = 'odom_filtered_input_scan'
  
  nodes = [
    # Lidar deskewing
@@ -124,24 +143,19 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
      parameters=[{
        'use_sim_time': use_sim_time,
        'fixed_frame_id': fixed_frame_id,
-        'wait_for_transform': 0.2}],
+        'wait_for_transform': 0.2,
+        'slerp': deskewing_slerp}],
      remappings=[
          ('input_cloud', lidar_topic)
      ]),
    
-    # Lidar odometry
-    Node(
-      package='rtabmap_odom', executable='icp_odometry', output='screen',
-      parameters=[shared_parameters, icp_odometry_parameters],
-      remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
-    
    # Assemble deskewed scans based on icp odometry
    Node(
      package='rtabmap_util', executable='point_cloud_assembler', output='screen',
      parameters=[{
        'use_sim_time': use_sim_time,
        'assembling_time': LaunchConfiguration('assembling_time'), 
-        'fixed_frame_id': ""}], # This will make the node subscribing to icp odometry topic "odom"
+        'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom"
      remappings=[('cloud', lidar_topic_deskewed),
                  ('odom', 'icp_odom')]),
    
@@ -150,18 +164,27 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
      package='rtabmap_slam', executable='rtabmap', output='screen',
      parameters=[shared_parameters, rtabmap_parameters,
                  {'subscribe_rgbd': rgbd_image_used,
-                   'topic_queue_size': 30,
-                   'sync_queue_size': 20,}],
-      remappings=remappings + [('scan_cloud', 'assembled_cloud')],
+                   'rgbd_cameras': rgbd_cameras,
+                   'topic_queue_size': 40,
+                   'sync_queue_size': 40,}],
+      remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
      arguments=arguments), 

    # Just for visualization
    Node(
      package='rtabmap_viz', executable='rtabmap_viz', output='screen',
      parameters=[shared_parameters, rtabmap_parameters],
-      remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
+      remappings=remappings + [('scan_cloud', viz_topic)])
  ]
  
+  if not external_odom_frame_id:
+    # Lidar odometry
+    nodes.append(
+      Node(
+        package='rtabmap_odom', executable='icp_odometry', output='screen',
+        parameters=[shared_parameters, icp_odometry_parameters],
+        remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]))
+  
  if fixed_frame_from_imu:
    # Create a stabilized base frame based on imu for lidar deskewing
    nodes.append(
@@ -190,7 +213,11 @@ def generate_launch_description():
    
    DeclareLaunchArgument(
      'fixed_frame_id', default_value='',
-      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
+      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'),
+    
+    DeclareLaunchArgument(
+      'external_odom_frame_id', default_value='',
+      description='Provide external odometry with TF, disabling icp_odometry.'),
    
    DeclareLaunchArgument(
      'localization', default_value='false',
@@ -204,18 +231,38 @@ def generate_launch_description():
      'imu_topic', default_value='/imu/data',
      description='Name of an IMU topic.'),
    
+    DeclareLaunchArgument(
+      'gps_topic', default_value='/gps/fix',
+      description='Name of a GPS topic.'),
+    
    DeclareLaunchArgument(
      'rgbd_image_topic', default_value='',
      description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
    
+    DeclareLaunchArgument(
+      'rgbd_images_topic', default_value='',
+      description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
+    
    DeclareLaunchArgument(
      'voxel_size', default_value='0.1',
      description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
    
+    DeclareLaunchArgument(
+      'min_loop_closure_overlap', default_value='0.2',
+      description='Minimum scan overlap pourcentage to accept a loop closure.'),
+    
+    DeclareLaunchArgument(
+      'expected_update_rate', default_value='15.0',
+      description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
+    
    DeclareLaunchArgument(
      'assembling_time', default_value='1.0',
      description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),

+    DeclareLaunchArgument(
+      'deskewing_slerp', default_value='true',
+      description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),
+
    DeclareLaunchArgument(
      'qos', default_value='1',
      description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
@@ -0,0 +1,305 @@
+# Description:
+#   In this example, we will record ALL lidar scans from 2 lidars. An IMU or low latency odometry is required for this example.
+# 
+# Example:
+#   Launch your lidar sensors
+#     In this example, we assume the lidar topics have a frame_id linked to same parent (e.g., base_link) and
+#     the extrinsics are known (URDF) and/or already calibrated.
+#   
+#   Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated.
+#     In this example, we assume the imu topic has 
+#     already the orientation estimated, if not, you can launch 
+#     imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
+#     and set imu_topic to output topic of the filter.
+#
+#   If a camera is used, make sure TF between lidar/base frame and camera is
+#     already calibrated. To provide image data to this example, you should use
+#     rtabmap_sync's rgbd_sync or stereo_sync node.
+#
+#   Launch the example by adjusting the lidar topics, imu topic and base frame:
+#   $ ros2 launch rtabmap_examples lidar3d.launch.py lidar1_topic:=/lidar1/velodyne_points lidar2_topic:=/lidar1/velodyne_points imu_topic:=/imu/data frame_id:=base_link
+
+from launch import LaunchDescription, LaunchContext
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
+  
+  frame_id = LaunchConfiguration('frame_id')
+  
+  external_odom_frame_id =  LaunchConfiguration('external_odom_frame_id').perform(context)
+
+  fixed_frame_from_imu = False
+  fixed_frame_id =  LaunchConfiguration('fixed_frame_id').perform(context)
+  if not fixed_frame_id:
+    if external_odom_frame_id:
+      fixed_frame_id = external_odom_frame_id
+    else:
+      fixed_frame_from_imu = True
+      fixed_frame_id = frame_id.perform(context) + "_stabilized"
+  
+  imu_topic = LaunchConfiguration('imu_topic')
+  
+  rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
+  rgbd_images_topic = LaunchConfiguration('rgbd_images_topic')
+  rgbd_image_used =  rgbd_image_topic.perform(context) != '' or rgbd_images_topic.perform(context) != ''
+  rgbd_cameras = 0 if rgbd_images_topic.perform(context) != '' else 1
+  
+  lidar1_topic = LaunchConfiguration('lidar1_topic')
+  lidar1_topic_value = lidar1_topic.perform(context)
+  lidar1_topic_deskewed = lidar1_topic_value + "/deskewed"
+  
+  lidar2_topic = LaunchConfiguration('lidar2_topic')
+  lidar2_topic_value = lidar2_topic.perform(context)
+  lidar2_topic_deskewed = lidar2_topic_value + "/deskewed"
+  
+  voxel_size = LaunchConfiguration('voxel_size')
+  voxel_size_value = float(voxel_size.perform(context))
+  
+  use_sim_time = LaunchConfiguration('use_sim_time')
+  
+  localization = LaunchConfiguration('localization').perform(context)
+  localization = localization == 'true' or localization == 'True'
+  
+  deskewing_slerp = LaunchConfiguration('deskewing_slerp').perform(context)
+  deskewing_slerp = deskewing_slerp == 'true' or deskewing_slerp == 'True'
+  
+  # Rule of thumb:
+  max_correspondence_distance = voxel_size_value * 10.0
+
+  shared_parameters = {
+    'use_sim_time': use_sim_time,
+    'frame_id': frame_id,
+    'qos': LaunchConfiguration('qos'),
+    'approx_sync': rgbd_image_used,
+    'wait_for_transform': 0.2,
+    # RTAB-Map's internal parameters are strings:
+    'Icp/PointToPlane': 'true',
+    'Icp/Iterations': '10',
+    'Icp/VoxelSize': str(voxel_size_value),
+    'Icp/Epsilon': '0.001',
+    'Icp/PointToPlaneK': '20',
+    'Icp/PointToPlaneRadius': '0',
+    'Icp/MaxTranslation': '3',
+    'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
+    'Icp/Strategy': '1',
+    'Icp/OutlierRatio': '0.7',
+  }
+
+  icp_odometry_parameters = {
+    'expected_update_rate': LaunchConfiguration('expected_update_rate'),
+    'wait_imu_to_init': True,
+    'odom_frame_id': 'icp_odom',
+    'guess_frame_id': fixed_frame_id,
+    # RTAB-Map's internal parameters are strings:
+    'Odom/ScanKeyFrameThr': '0.4',
+    'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
+    'OdomF2M/ScanMaxSize': '15000',
+    'OdomF2M/BundleAdjustment': 'false',
+    'Icp/CorrespondenceRatio': '0.01'
+  }
+
+  rtabmap_parameters = {
+    'subscribe_depth': False,
+    'subscribe_rgb': False,
+    'subscribe_odom_info': not external_odom_frame_id,
+    'subscribe_scan_cloud': True,
+    'odom_frame_id': (external_odom_frame_id if external_odom_frame_id else ""),
+    'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
+    # RTAB-Map's internal parameters are strings:
+    'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
+    'RGBD/ProximityMaxGraphDepth': '0',
+    'RGBD/ProximityPathMaxNeighbors': '1',
+    'RGBD/AngularUpdate': '0.05',
+    'RGBD/LinearUpdate': '0.05',
+    'RGBD/CreateOccupancyGrid': 'false',
+    'Mem/NotLinkedNodesKept': 'false',
+    'Mem/STMSize': '30',
+    'Reg/Strategy': '1',
+    'Icp/CorrespondenceRatio': str(LaunchConfiguration('min_loop_closure_overlap').perform(context))
+  }
+  
+  remappings = [('imu', imu_topic),
+                ('odom', 'icp_odom')]
+  if rgbd_image_used:
+    if rgbd_cameras == 1:
+      remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
+    else:
+      remappings.append(('rgbd_images', LaunchConfiguration('rgbd_images_topic')))
+    
+  arguments = []
+  if localization:
+    rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
+    rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
+  else:
+    arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+  
+  if external_odom_frame_id:
+    viz_topic = "combined_cloud"
+  else:
+    viz_topic = 'odom_filtered_input_scan'
+  
+  nodes = [
+    # Lidar1 deskewing
+    Node(
+      package='rtabmap_util', executable='lidar_deskewing', name="lidar1_deskewing", output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'fixed_frame_id': fixed_frame_id,
+        'wait_for_transform': 0.2,
+        'slerp': deskewing_slerp}],
+      remappings=[
+          ('input_cloud', lidar1_topic)
+      ]),
+
+    # Lidar2 deskewing
+    Node(
+      package='rtabmap_util', executable='lidar_deskewing', name="lidar2_deskewing", output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'fixed_frame_id': fixed_frame_id,
+        'wait_for_transform': 0.2,
+        'slerp': deskewing_slerp}],
+      remappings=[
+          ('input_cloud', lidar2_topic)
+      ]),
+
+    # Combine the two lidars in single point cloud
+    Node(
+      package='rtabmap_util', executable='point_cloud_aggregator', output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'approx_sync': True,
+        'fixed_frame_id': fixed_frame_id,
+        'count': 2}],
+      remappings=[
+        ('cloud1', lidar1_topic_deskewed),
+        ('cloud2', lidar2_topic_deskewed)]),
+    
+    # Assemble combined deskewed scans based on icp odometry
+    Node(
+      package='rtabmap_util', executable='point_cloud_assembler', output='screen',
+      parameters=[{
+        'use_sim_time': use_sim_time,
+        'assembling_time': LaunchConfiguration('assembling_time'), 
+        'fixed_frame_id': (external_odom_frame_id if external_odom_frame_id else "")}], # This will make the node subscribing to icp odometry topic "icp_odom"
+      remappings=[('cloud', "combined_cloud"),
+                  ('odom', 'icp_odom')]),
+    
+    # Update the map
+    Node(
+      package='rtabmap_slam', executable='rtabmap', output='screen',
+      parameters=[shared_parameters, rtabmap_parameters,
+                  {'subscribe_rgbd': rgbd_image_used,
+                   'rgbd_cameras': rgbd_cameras,
+                   'topic_queue_size': 40,
+                   'sync_queue_size': 40,}],
+      remappings=remappings + [('scan_cloud', 'assembled_cloud'), ('gps/fix', LaunchConfiguration('gps_topic'))],
+      arguments=arguments), 
+
+    # Just for visualization
+    Node(
+      package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+      parameters=[shared_parameters, rtabmap_parameters],
+      remappings=remappings + [('scan_cloud', viz_topic)])
+  ]
+  
+  if not external_odom_frame_id:
+    # Lidar odometry
+    nodes.append(
+      Node(
+        package='rtabmap_odom', executable='icp_odometry', output='screen',
+        parameters=[shared_parameters, icp_odometry_parameters],
+        remappings=remappings + [('scan_cloud', "combined_cloud")]))
+  
+  if fixed_frame_from_imu:
+    # Create a stabilized base frame based on imu for lidar deskewing
+    nodes.append(
+      Node(
+        package='rtabmap_util', executable='imu_to_tf', output='screen',
+        parameters=[{
+          'use_sim_time': use_sim_time,
+          'fixed_frame_id': fixed_frame_id,
+          'base_frame_id': frame_id,
+          'wait_for_transform_duration': 0.001}],
+        remappings=[('imu/data', imu_topic)]))
+  
+  return nodes
+  
+def generate_launch_description():
+  return LaunchDescription([
+
+    # Launch arguments
+    DeclareLaunchArgument(
+      'use_sim_time', default_value='false',
+      description='Use simulated clock.'),
+    
+    DeclareLaunchArgument(
+      'frame_id', default_value='velodyne',
+      description='Base frame of the robot.'),
+    
+    DeclareLaunchArgument(
+      'fixed_frame_id', default_value='',
+      description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU or external_odom_frame_id if not null.'),
+    
+    DeclareLaunchArgument(
+      'external_odom_frame_id', default_value='',
+      description='Provide external odometry with TF, disabling icp_odometry.'),
+    
+    DeclareLaunchArgument(
+      'localization', default_value='false',
+      description='Localization mode.'),
+
+    DeclareLaunchArgument(
+      'lidar1_topic', default_value='/lidar1/velodyne_points',
+      description='Name of the lidar1\'s PointCloud2 topic.'),
+    
+    DeclareLaunchArgument(
+      'lidar2_topic', default_value='/lidar2/velodyne_points',
+      description='Name of the lidar2\'s PointCloud2 topic.'),
+
+    DeclareLaunchArgument(
+      'imu_topic', default_value='/imu/data',
+      description='Name of an IMU topic.'),
+    
+    DeclareLaunchArgument(
+      'gps_topic', default_value='/gps/fix',
+      description='Name of a GPS topic.'),
+    
+    DeclareLaunchArgument(
+      'rgbd_image_topic', default_value='',
+      description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
+    
+    DeclareLaunchArgument(
+      'rgbd_images_topic', default_value='',
+      description='RGBD images topic (ignored if empty, override "rgbd_image_topic" if set). Would be the output of a rtabmap_sync\'s rgbdx_sync node.'),
+    
+    DeclareLaunchArgument(
+      'voxel_size', default_value='0.1',
+      description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
+    
+    DeclareLaunchArgument(
+      'min_loop_closure_overlap', default_value='0.2',
+      description='Minimum scan overlap pourcentage to accept a loop closure.'),
+    
+    DeclareLaunchArgument(
+      'expected_update_rate', default_value='15.0',
+      description='Expected lidar frame rate. Ideally, set it slightly higher than actual frame rate, like 15 Hz for 10 Hz lidar scans.'),
+    
+    DeclareLaunchArgument(
+      'assembling_time', default_value='1.0',
+      description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
+
+    DeclareLaunchArgument(
+      'deskewing_slerp', default_value='true',
+      description='Use fast slerp interpolation between first and last stamps of the scan for deskewing. It would less accruate than requesting TF for every points, but a lot faster. Enable this if the delay of the deskewed scan is significant larger than the original scan.'),
+
+    DeclareLaunchArgument(
+      'qos', default_value='1',
+      description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
+
+    OpaqueFunction(function=launch_setup),
+  ])
+
+    
@@ -1,47 +1,72 @@

 # Example to run rgbd datasets:
+#
 # [ROS1] Prepare ROS1 rosbag for conversion to ROS2
 #   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
 #   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
+#   $ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/45e6ac01541973a17505000fc8c7ca399ae275eb/tum_rename_world_kinect_frame.py
+#   $ python3 tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag
 #   $ wget https://raw.githubusercontent.com/srv/srv_tools/kinetic/bag_tools/scripts/change_frame_id.py
+#
 #   Edit change_frame_id.py, remove/comment lines beginning with "PKG" and "import roslib", change line "Exception, e" to "Exception"
 #   $ roscore
 #   $ python3 change_frame_id.py -o rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag -i rgbd_dataset_freiburg3_long_office_household.bag -f openni_rgb_optical_frame -t /camera/rgb/image_color
+#
 # [ROS2]
 #   $ sudo pip install rosbags     # See https://docs.openvins.com/dev-ros1-to-ros2.html
-#   $ rosbags-convert rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag
-
+#   $ rosbags-convert --src rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag --dst rgbd_dataset_freiburg3_long_office_household_frameid_fixed
+#
 #   $ ros2 launch rtabmap_examples rgbdslam_datasets.launch.py
 #   $ cd rgbd_dataset_freiburg3_long_office_household_frameid_fixed
 #   $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 --clock
-
+#
+# To get RMSE after the run:
+#   $ rtabmap-report ~/.ros/rtabmap.db

 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.actions import SetParameter

 def generate_launch_description():

-    parameters=[{
+    odom_parameters=[{
+        'frame_id':'kinect',
+        # ground truth here is just used to align odometry with ground truth's first pose
+        'ground_truth_frame_id':'world',
+        'ground_truth_base_frame_id':'kinect_gt',
+        'keep_color': True,
+        'wait_for_transform': 0.5,
+        # RTAB-Map's parameters should all be string type:
+        'Odom/Strategy':'0',
+        'Odom/ResetCountdown':'15',
+        'Odom/GuessSmoothingDelay':'0',
+    }]
+    slam_parameters=[{
          'frame_id':'kinect',
-          'subscribe_depth':True,
+          # Record ground truth to compute RMSE
+          'ground_truth_frame_id':'world',
+          'ground_truth_base_frame_id':'kinect_gt',
+          'subscribe_rgb':False,
+          'subscribe_depth':False,
+          'subscribe_rgbd':True,
          'subscribe_odom_info':True,
          # RTAB-Map's parameters should all be string type:
-          'Odom/Strategy':'0',
-          'Odom/ResetCountdown':'15',
-          'Odom/GuessSmoothingDelay':'0',
+          'Mem/UseOdomFeatures': 'true',
          'Rtabmap/StartNewMapOnLoopClosure':'true',
          'RGBD/CreateOccupancyGrid':'false',
          'Rtabmap/CreateIntermediateNodes':'true',
          'RGBD/LinearUpdate':'0',
          'RGBD/AngularUpdate':'0'}]
          
-    remappings=[
+    odom_remappings=[
          ('rgb/image', '/camera/rgb/image_color'),
          ('rgb/camera_info', '/camera/rgb/camera_info'),
          ('depth/image', '/camera/depth/image')]
+    
+    # We will use the output of odometry to avoid re-extracting 
+    # the same features on slam side.
+    slam_remappings=[
+        ("rgbd_image", "odom_rgbd_image")]
          

    return LaunchDescription([
@@ -52,19 +77,19 @@ def generate_launch_description():
        # Nodes to launch
        Node(
            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
-            parameters=parameters,
-            remappings=remappings),
+            parameters=odom_parameters,
+            remappings=odom_remappings),

        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=parameters,
-            remappings=remappings,
+            parameters=slam_parameters,
+            remappings=slam_remappings,
            arguments=['-d']),

        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=parameters,
-            remappings=remappings),
+            parameters=slam_parameters,
+            remappings=slam_remappings),
       
        # /tf topic is missing in the converted ROS2 bag, create a fake tf
        Node(
@@ -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.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's example launch files.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -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.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's main launch files.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -18,6 +18,8 @@ int32 local_key_frames
 int32 local_bundle_outliers
 int32 local_bundle_constraints
 float32 local_bundle_time
+float32 local_bundle_avg_inlier_distance
+int32 local_bundle_max_key_frames_for_inlier
 bool key_frame_added
 float32 time_estimation
 float32 time_particle_filtering
@@ -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.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's msgs package.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -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.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's odometry package.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -113,6 +113,8 @@ 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 depthTransport = this->declare_parameter("depth_transport", std::string("raw"));

 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
 	if(approxSync)
@@ -124,6 +126,8 @@ 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: depth_transport = %s", depthTransport.c_str());

 	rclcpp::SubscriptionOptions options;
 	options.callback_group = dataCallbackGroup_;
@@ -350,10 +354,19 @@ void RGBDOdometry::onOdomInit()
 	}
 	else
 	{
-		image_transport::TransportHints hints(this);
-		image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
-		image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
-		info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
+		image_transport::TransportHints rgb_hints(this, "raw", "rgb_transport");
+		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);
+		info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());

 		if(approxSync)
 		{
@@ -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_python</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's python package.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -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_ros</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>
    RTAB-Map Stack
  </description>
@@ -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_rviz_plugins</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's rviz plugins.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -57,6 +57,10 @@ SET(Libraries
   rtabmap_sync
 )

+if("$ENV{ROS_DISTRO}" STRLESS "jazzy")
+  add_definitions(-DPRE_ROS_JAZZY)
+endif()
+
 ###########
 ## Build ##
 ###########
@@ -405,10 +405,15 @@ private:
 	cv::Mat userData_;
 	UMutex userDataMutex_;

+	rclcpp::CallbackGroup::SharedPtr globalPoseAsyncCallbackGroup_;
 	rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPoseAsyncSub_;
-	geometry_msgs::msg::PoseWithCovarianceStamped globalPose_;
+	std::map<double, geometry_msgs::msg::PoseWithCovarianceStamped> globalPoses_;
+	UMutex globalPoseMutex_;
+
+	rclcpp::CallbackGroup::SharedPtr gpsAsyncCallbackGroup_;
 	rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixAsyncSub_;
-	rtabmap::GPS gps_;
+	std::map<double, rtabmap::GPS> gps_;
+	UMutex gpsMutex_;

 	rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_;
 	rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;
@@ -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_slam</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's SLAM package.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -90,6 +90,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap_conversions/MsgConversion.h"

+#ifdef PRE_ROS_JAZZY
+namespace rclcpp {
+	rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;}
+	rmw_qos_profile_t ParametersQoS() {return rmw_qos_profile_parameters;}
+}
+#endif
+
 using namespace rtabmap;

 namespace rtabmap_slam {
@@ -145,7 +152,6 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	char * rosHomePath = getenv("ROS_HOME");
 	std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
 	databasePath_ = workingDir+"/"+rtabmap::Parameters::getDefaultDatabaseName();
-	globalPose_.header.stamp = rclcpp::Time(0);

 	mapsManager_.init(*this, this->get_name(), true);

@@ -358,7 +364,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		std::string vStr = this->declare_parameter(iter->first, iter->second);
 		if(overrides.find(iter->first) != overrides.end())
 		{
-			RCLCPP_INFO(this->get_logger(), "Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			RCLCPP_INFO(this->get_logger(), "Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), vStr.c_str());

 			if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
 			{
@@ -658,45 +664,45 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :

 	// setup services
 	const std::string servicePrefix = get_name() + std::string("/");
-	updateSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	backupDatabase_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
+	updateSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	backupDatabase_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);

 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
-	octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
+	octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
 #endif
 #endif
 	//private services
-	setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
-	setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_);
+	setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);
+	setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rclcpp::ServicesQoS(), processingCallbackGroup_);

 	int optimizeIterations = 0;
 	Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
@@ -721,7 +727,17 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 					tfBroadcaster_->sendTransform(msg);
 				}
 				mapToOdomMutex_.unlock();
-				r.sleep();
+				try {
+					r.sleep();
+				}
+				catch(std::exception & e) {
+					if(rclcpp::ok()) {
+						RCLCPP_ERROR(this->get_logger(),
+							"Could not sleep: \"%s\", TF \"%s\"->\"%s\" won't be published anymore!",
+							e.what(), mapFrameId_.c_str(), odomFrameId_.c_str());
+					} // else: the node may have been shutdown
+					break;
+				}
 			}
 		});
 	}
@@ -844,12 +860,18 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :

 	// Setup callback groups for any subscriptions that should not be affected by main processing thread.
 	userDataAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+	globalPoseAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+	gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	rclcpp::SubscriptionOptions userDataAsyncSubOptions;
+	rclcpp::SubscriptionOptions globalPoseAsyncSubOptions;
+	rclcpp::SubscriptionOptions gpsAsyncSubOptions;
 	rclcpp::SubscriptionOptions landmarkSubOptions;
 	rclcpp::SubscriptionOptions imuSubOptions;
 	userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_;
+	globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_;
+	gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_;
 	landmarkSubOptions.callback_group = imuCallbackGroup_;
 	imuSubOptions.callback_group = imuCallbackGroup_;

@@ -858,8 +880,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	qosGPS = this->declare_parameter("qos_gps", qosGPS);
 	qosIMU = this->declare_parameter("qos_imu", qosIMU);
 	userDataAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::UserData>("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions);
-	globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), subOptions);
-	gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), subOptions);
+	globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions);
+	gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), gpsAsyncSubOptions);
 	landmarkDetectionSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
 	landmarkDetectionsSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetections>("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
 #ifdef WITH_APRILTAG_MSGS
@@ -870,8 +892,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 #endif
 	imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1), imuSubOptions);
 	republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(servicePrefix+"republish_node_data", 1, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1), subOptions);
+	parametersClient_ = std::make_shared<rclcpp::AsyncParametersClient>(this, std::string(), rclcpp::ParametersQoS(), processingCallbackGroup_);

-	parametersClient_ = std::make_shared<rclcpp::AsyncParametersClient>(this, std::string(), rmw_qos_profile_parameters, processingCallbackGroup_);
 	auto on_parameter_event_callback =
 			[this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void
 			{
@@ -922,7 +944,10 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 						RCLCPP_INFO(this->get_logger(), "RTAB-Map rate detection = %f Hz", rate_);
 					}
 					rtabmap_.parseParameters(parameters_);
-					mapsManager_.setParameters(parameters_);
+					// Don't reset map in localization mode
+					if(rtabmap_.getMemory()->isIncremental()) {
+						mapsManager_.setParameters(parameters_);
+					}
 				}
 			};

@@ -2056,18 +2081,40 @@ void CoreWrapper::process(
 		data.setGroundTruth(groundTruthPose);

 		//global pose
-		if(globalPose_.header.stamp.sec != 0 || globalPose_.header.stamp.nanosec != 0)
+		geometry_msgs::msg::PoseWithCovarianceStamped globalPoseMsg;
+		globalPoseMsg.header.stamp = rclcpp::Time(0);
+		{
+			UScopeMutex lock(globalPoseMutex_);
+			if(!globalPoses_.empty())
+			{
+				auto iter = rtabmap_conversions::getClosestIterator<double, geometry_msgs::msg::PoseWithCovarianceStamped>(globalPoses_, data.stamp());
+				// Check if it is not too old
+				if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_)
+				{
+					globalPoseMsg = iter->second;
+				}
+				else
+				{
+					RCLCPP_WARN(this->get_logger(), "Ignoring global pose with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).",
+						iter->first,
+						1.0/rate_,
+						data.stamp());
+				}
+				globalPoses_.clear();
+			}
+		}
+		if(globalPoseMsg.header.stamp.sec != 0 || globalPoseMsg.header.stamp.nanosec != 0)
 		{
 			// assume sensor is fixed
 			Transform sensorToBase = rtabmap_conversions::getTransform(
-					globalPose_.header.frame_id,
+					globalPoseMsg.header.frame_id,
 					frameId_,
 					stamp,
 					*tfBuffer_,
 					waitForTransform_);
 			if(!sensorToBase.isNull())
 			{
-				Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPose_.pose.pose);
+				Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPoseMsg.pose.pose);
 				globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame

 				// Correction of the global pose accounting the odometry movement since we received it
@@ -2075,7 +2122,7 @@ void CoreWrapper::process(
 						frameId_,
 						odomFrameId,
 						stamp,
-						rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec),
+						rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec),
 						*tfBuffer_,
 						waitForTransform_);
 				if(!correction.isNull())
@@ -2088,17 +2135,32 @@ void CoreWrapper::process(
 							"If odometry is small since it received the global pose and "
 							"covariance is large, this should not be a problem.");
 				}
-				cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
+				cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPoseMsg.pose.covariance.data()).clone();
 				data.setGlobalPose(globalPose, globalPoseCovariance);
 			}
 		}
-		globalPose_.header.stamp = rclcpp::Time(0);

-		if(gps_.stamp() > 0.0)
 		{
-			data.setGPS(gps_);
+			UScopeMutex lock(gpsMutex_);
+			if(!gps_.empty())
+			{
+				std::map<double, rtabmap::GPS>::const_iterator iter = rtabmap_conversions::getClosestIterator<double, rtabmap::GPS>(gps_, data.stamp());
+				// Check if it is not too old
+				if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_)
+				{
+					data.setGPS(iter->second);
+				}
+				else
+				{
+					RCLCPP_WARN(this->get_logger(), "Ignoring GPS with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).",
+						iter->first,
+						1.0/rate_,
+						data.stamp());
+				}
+				gps_.clear();
+			}
 		}
-		gps_ = rtabmap::GPS();
+		

 		//tag detections
 		landmarksMutex_.lock();
@@ -2517,7 +2579,12 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCova
 {
 	if(!paused_)
 	{
-		globalPose_ = *globalPoseMsg;
+		UScopeMutex lock(globalPoseMutex_);
+		globalPoses_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(globalPoseMsg->header.stamp), *globalPoseMsg));
+		if(globalPoses_.size() > 1000)
+		{
+			globalPoses_.erase(globalPoses_.begin());
+		}
 	}
 }

@@ -2534,13 +2601,21 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP
 				error = sqrt(variance);
 			}
 		}
-		gps_ = rtabmap::GPS(
-				rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp),
-				gpsFixMsg->longitude,
-				gpsFixMsg->latitude,
-				gpsFixMsg->altitude,
-				error,
-				0);
+
+		rtabmap::GPS gps(
+			rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp),
+			gpsFixMsg->longitude,
+			gpsFixMsg->latitude,
+			gpsFixMsg->altitude,
+			error,
+			0);
+
+		UScopeMutex lock(gpsMutex_);
+		gps_.insert(std::make_pair(gps.stamp(), gps));
+		if(gps_.size() > 1000)
+		{
+			gps_.erase(gps_.begin());
+		}
 	}
 }

@@ -2693,14 +2768,41 @@ void CoreWrapper::interOdomInfoCallback(const nav_msgs::msg::Odometry::ConstShar

 void CoreWrapper::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 {
-	Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
-	if(intialPose.isNull())
+	Transform mapToPose = Transform::getIdentity();
+	if(msg->header.frame_id.empty())
 	{
-		RCLCPP_ERROR(this->get_logger(), "Pose received is null!");
-		return;
+		RCLCPP_WARN(this->get_logger(), "Received initialpose doesn't have frame_id set, assuming it is in %s frame.", mapFrameId_.c_str());
+	}
+	else if(msg->header.frame_id != mapFrameId_)
+	{
+		mapToPose = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
+		if(mapToPose.isNull())
+		{
+			RCLCPP_ERROR(this->get_logger(), "Failed to transform initialpose from frame %s to map frame %s", msg->header.frame_id.c_str(), mapFrameId_.c_str());
+			return;
+		}
 	}

-	rtabmap_.setInitialPose(intialPose);
+	Transform initialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
+	if(initialPose.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "initialpose received is null!");
+		return;
+	}
+	if(mapToPose.isIdentity())
+	{
+		RCLCPP_INFO(this->get_logger(), "initialpose received: %s", initialPose.prettyPrint().c_str());
+		rtabmap_.setInitialPose(initialPose);
+	}
+	else
+	{
+		RCLCPP_INFO(this->get_logger(), "initialpose received: %s in %s frame, transformed to %s in %s frame.",
+			initialPose.prettyPrint().c_str(),
+			msg->header.frame_id.c_str(),
+			(mapToPose * initialPose).prettyPrint().c_str(),
+			mapFrameId_.c_str());
+		rtabmap_.setInitialPose(mapToPose*initialPose);
+	}
 }

 void CoreWrapper::goalCommonCallback(
@@ -2944,7 +3046,10 @@ void CoreWrapper::updateRtabmapCallback(
 		RCLCPP_INFO(get_logger(), "2D mapping = %s", twoDMapping_?"true":"false");
 	}
 	rtabmap_.parseParameters(parameters_);
-	mapsManager_.setParameters(parameters_);
+	// Don't reset map in localization mode
+	if(rtabmap_.getMemory()->isIncremental()) {
+		mapsManager_.setParameters(parameters_);
+	}
 }

 void CoreWrapper::resetRtabmapCallback(
@@ -2970,8 +3075,8 @@ void CoreWrapper::resetRtabmapCallback(
 	graphLatched_ = false;
 	mapsManager_.clear();
 	previousStamp_ = rclcpp::Time(0);
-	globalPose_.header.stamp = rclcpp::Time(0);
-	gps_ = rtabmap::GPS();
+	globalPoses_.clear();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();
@@ -3072,8 +3177,8 @@ void CoreWrapper::loadDatabaseCallback(
 	graphLatched_ = false;
 	mapsManager_.clear();
 	previousStamp_ = rclcpp::Time(0);
-	globalPose_.header.stamp = rclcpp::Time(0);
-	gps_ = rtabmap::GPS();
+	globalPoses_.clear();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();
@@ -3092,14 +3197,16 @@ void CoreWrapper::loadDatabaseCallback(
 	// Open new database
 	databasePath_ = newDatabasePath;

-	// modify default parameters with those in the database
+	//Warn if database's parameters are different than the current ones we are using
 	if(!req->clear && UFile::exists(databasePath_))
 	{
 		ParametersMap dbParameters;
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
+		std::string databaseVersion = "0.0.0";
 		if(driver->openConnection(databasePath_))
 		{
 			dbParameters = driver->getLastParameters(); // parameter migration is already done
+			databaseVersion = driver->getDatabaseVersion();
 		}
 		delete driver;
 		for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter)
@@ -3109,15 +3216,31 @@ void CoreWrapper::loadDatabaseCallback(
 				// ignore working directory
 				continue;
 			}
-			if(parameters_.find(iter->first) == parameters_.end() &&
-				parameters_.find(iter->first)->second.compare(iter->second) !=0)
+			if(iter->first.find("Odom") == 0)
 			{
-				RCLCPP_WARN(get_logger(), "RTAB-Map parameter \"%s\" from database (%s) is different "
-						"from the current used one (%s). We still keep the "
+				// ignore odometry params
+				continue;
+			}
+			if(parameters_.find(iter->first) == parameters_.end())
+			{
+				RCLCPP_WARN(get_logger(), "RTAB-Map parameter \"%s\" from database (%s, version \"%s\") doesn't exist "
+						"in current rtabmap version (\"%s\"). The parameter is ignored.",
+						iter->first.c_str(),
+						iter->second.c_str(),
+						databaseVersion.c_str(),
+						RTABMAP_VERSION);
+			}
+			else if(parameters_.find(iter->first)->second.compare(iter->second) !=0)
+			{
+				RCLCPP_WARN(get_logger(), "RTAB-Map parameter \"%s\" from database (%s, version=\"%s\") is different "
+						"from the current used one (%s, version=\"%s\"). We still keep the "
 						"current parameter value (%s). If you want to switch between databases "
 						"with different configurations, restart rtabmap node instead of using this service.",
-						iter->first.c_str(), iter->second.c_str(),
+						iter->first.c_str(),
+						iter->second.c_str(),
+						databaseVersion.c_str(),
 						parameters_.find(iter->first)->second.c_str(),
+						RTABMAP_VERSION,
 						parameters_.find(iter->first)->second.c_str());
 			}
 		}
@@ -3208,8 +3331,8 @@ void CoreWrapper::backupDatabaseCallback(
 	userDataMutex_.lock();
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
-	globalPose_.header.stamp = rclcpp::Time(0);
-	gps_ = rtabmap::GPS();
+	globalPoses_.clear();
+	gps_.clear();
 	landmarksMutex_.lock();
 	landmarks_.clear();
 	landmarksMutex_.unlock();
@@ -29,10 +29,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_

 #include <rtabmap_sync/visibility.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.hpp>
+#include <message_filters/synchronizer.hpp>
+#include <message_filters/sync_policies/approximate_time.hpp>
+#include <message_filters/sync_policies/exact_time.hpp>

 #include <image_transport/image_transport.hpp>
 #include <image_transport/subscriber_filter.hpp>
@@ -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_sync</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's synchronization package.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -56,6 +56,10 @@ SET(Libraries
   rtabmap_conversions
 )

+if("$ENV{ROS_DISTRO}" STRLESS "jazzy")
+  add_definitions(-DPRE_ROS_JAZZY)
+endif()
+
 ###########
 ## Build ##
 ###########
@@ -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_util</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's various useful nodes and nodelets.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_conversions/MsgConversion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2/LinearMath/Transform.h>
+#include <tf2/utils.hpp>

 namespace rtabmap_util
 {
@@ -63,8 +64,7 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
 {
 	tf2::Quaternion q;
 	tf2::fromMsg(msg->orientation, q);
-	tf2::Transform st;
-	st.setRotation(q);
+	tf2::Transform st(q);

 	std::string childFrameId = msg->header.frame_id;

@@ -81,10 +81,12 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
 				return;
 			}

-			geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp);
+			geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp);
 			tf2::Transform tmp_t;
 			tf2::fromMsg(tmp.transform, tmp_t);
-			tf2::Transform t = tmp_t.inverse()*st*tmp_t;
+			tf2::Quaternion q;
+			q.setRPY(0.0,0.0,tf2::getYaw(tmp_t.getRotation()));
+			tf2::Transform t = tf2::Transform(q)*st*tmp_t.inverse(); // base_frame orientation
 			st.setRotation(t.getRotation());
 			childFrameId = baseFrameId_;
 		}
@@ -94,7 +96,6 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
 			return;
 		}
 	}
-	st.setOrigin(tf2::Vector3(0,0,0));

 	geometry_msgs::msg::TransformStamped output;
 	output.header.frame_id = fixedFrameId_;
@@ -40,6 +40,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif

+#ifdef PRE_ROS_JAZZY
+namespace rclcpp{
+	rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;}
+}
+#endif
+
 using namespace std::chrono_literals;

 namespace rtabmap_util
@@ -187,7 +193,7 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 	// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
 	serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 	timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-	client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rmw_qos_profile_services_default, serviceCbGroup_); // Put it in a different group than the timer
+	client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
 	timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
 }

@@ -111,10 +111,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 				get_name(),
 				approx?"approx":"exact",
 				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
-				cloudSub_1_.getTopic().c_str(),
-				cloudSub_2_.getTopic().c_str(),
-				cloudSub_3_.getTopic().c_str(),
-				cloudSub_4_.getTopic().c_str());
+				cloudSub_1_.getSubscriber()->get_topic_name(),
+				cloudSub_2_.getSubscriber()->get_topic_name(),
+				cloudSub_3_.getSubscriber()->get_topic_name(),
+				cloudSub_4_.getSubscriber()->get_topic_name());
 	}
 	else if(count == 3)
 	{
@@ -135,9 +135,9 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 				this->get_name(),
 				approx?"approx":"exact",
 				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
-				cloudSub_1_.getTopic().c_str(),
-				cloudSub_2_.getTopic().c_str(),
-				cloudSub_3_.getTopic().c_str());
+				cloudSub_1_.getSubscriber()->get_topic_name(),
+				cloudSub_2_.getSubscriber()->get_topic_name(),
+				cloudSub_3_.getSubscriber()->get_topic_name());
 	}
 	else
 	{
@@ -157,8 +157,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 				this->get_name(),
 				approx?"approx":"exact",
 				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
-				cloudSub_1_.getTopic().c_str(),
-				cloudSub_2_.getTopic().c_str());
+				cloudSub_1_.getSubscriber()->get_topic_name(),
+				cloudSub_2_.getSubscriber()->get_topic_name());
 	}


@@ -175,10 +175,11 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 						this->get_name(),
 						approx?"":"Parameter \"approx_sync\" is false, which means that input "
 							"topics should have all the exact timestamp for the callback to be called.",
-							subscribedTopicsMsg.c_str());
+						subscribedTopicsMsg.c_str());
 			}
 		}
 	});
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 }

 PointCloudAggregator::~PointCloudAggregator()
@@ -154,9 +154,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 		exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
 							get_name(),
-							syncCloudSub_.getTopic().c_str(),
-							syncOdomSub_.getTopic().c_str(),
-							syncOdomInfoSub_.getTopic().c_str());
+							syncCloudSub_.getSubscriber()->get_topic_name(),
+							syncOdomSub_.getSubscriber()->get_topic_name(),
+							syncOdomInfoSub_.getSubscriber()->get_topic_name());
 	}
 	else
 	{
@@ -166,8 +166,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 		exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
 							get_name(),
-							syncCloudSub_.getTopic().c_str(),
-							syncOdomSub_.getTopic().c_str());
+							syncCloudSub_.getSubscriber()->get_topic_name(),
+							syncOdomSub_.getSubscriber()->get_topic_name());
 	}

 	warningThread_ = new std::thread([&](){
@@ -39,8 +39,15 @@ SET(Libraries
 ## Build ##
 ###########

-add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
+add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp include/${PROJECT_NAME}/PreferencesDialogROS.h)
 ament_target_dependencies(rtabmap_viz ${Libraries})
+SET_TARGET_PROPERTIES(
+  rtabmap_viz
+   PROPERTIES
+	   AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)

 #############
 ## Install ##
@@ -36,6 +36,7 @@ using namespace rtabmap;

 class PreferencesDialogROS : public PreferencesDialog
 {
+	Q_OBJECT
 public:
 	PreferencesDialogROS(rclcpp::Node * node, const QString & configFile, const std::string & rtabmapNodeName);
 	virtual ~PreferencesDialogROS();
@@ -44,7 +45,7 @@ public:
 	virtual QString getTmpIniFilePath() const;
 	bool hasAllParameters();

-public slots:
+public Q_SLOTS:
 	void readRtabmapNodeParameters();

 protected:
@@ -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_viz</name>
-  <version>0.21.10</version>
+  <version>0.22.0</version>
  <description>RTAB-Map's visualization package.</description>
  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>